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ABSTRACT 


Control  of  the  motions  and  vibrations  of  large  space  structures  require  the  know¬ 
ledge  of  state  values  that  may  not  be  available  due  either  to  inability  to  measure  the 
states  or,  the  high  cost  of  the  sensors  to  measure  the  required  states.  One  solution  is  the 
use  of  an  observer  to  estimate  the  states  from  limited  sensor  input. 

The  physical  characteristics  of  large  space  structures  and  the  environment  they  oper¬ 
ate  in  will  cause  large  amounts  of  noise  in  the  measurements.  The  obvious  observer  for 
such  an  environment  is  the  Kalman  Filter  which  is  specifically  designed  to  produce  opti¬ 
mal  estimates  in  a  noisy  environment. 

A  straightforward  application  of  the  Kalman  Filter  will  be  examined  utilizing  a 
steady  state  Kalman  gain  matrix.  The  observer  performance  will  be  examined  in  both 
matched  filter/plant  an4  reduced  order  filter  configurations. 
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The  reader  is  cautioned  that  computer  programs  developed  in  this  research  may  not 
have  been  exercised  for  all  cases  of  interest.  While  every  effort  has  been  made,  within 
the  time  available,  to  ensure  that  the  programs  are  free  of  computational  and  logic  er¬ 
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I.  INTRODUCTION 


A.  BACKGROUND 

The  advent  of  large  space  structures  poses  a  number  of  problems  for  the  control 
engineer.  Previously,  the  objects  put  into  space  could  be  treated  as  rigid  bodies  so  that 
a  single  three  axis  sensor  package  cou’d  be  used  to  tell  the  motion  of  all  components. 
The  large  space  structures  will  not  be  rigid,  instead  they  will  have  considerable  flexibility 
and  multiple  modes  of  vibration  [Ref.  1:  p.  51] 

Control  of  the  structure's  attitude  and  vibrations  requires  knowing  the  motions  of 
the  components.  One  approach  would  be  to  heavily  instrument  the  space  structure,  but 
weight  and  cost  make  this  approach  impractical.  An  alternative  is  to  use  a  limited 
number  of  sensors  to  measure  only  certain  states  and  to  deduce  the  other  required  states 
by  use  of  an  observer  algorithm. 

This  thesis  will  address  the  production  of  estimates  of  the  states  needed  for  control 
of  the  structure.  The  model  used  will  be  a  early  design  study  by  McDonnell  Douglas 
Astronautics  for  a  dual  keel  space  station.  The  techniques  and  problems  of  observation 
for  this  model  are  generic  to  all  large  space  structures. 

B.  PROBLEM  STATEMENT 

Design  of  an  observer  for  estimating  the  states  of  a  large  space  structure  breaks 
down  into  several  steps.  First,  a  matematical  model  is  developed  for  the  system  behavior 
over  time.  Modal  analysis  is  used  to  form  a  system  composed  of  decoupled  second  order 
differential  equations.  The  use  of  decoupled  equations  allows  a  reduced  order  model  to 
be  generated  by  truncating  the  number  of  modal  equations.  A  reduced  order  model  will 
have  all  of  the  same  mathematical  qualities  (and  problems)  but  reduces  the  amount  of 
time  and  computer  resources  required  to  do  simulation. 

Second,  the  observer  is  designed.  The  observer  is  designed  to  obtain  a  minimum 
variance  estimate  of  the  desired  state  values  from  the  measurements. 

Third,  the  observer  is  simulated  to  verify  performance.  Simulation  runs  of  both  a 
matched  observer/plant  system  and  a  reduced  order  observer  are  employed.  That  is,  the 
system  is  run  where  the  observer  is  used  to  estimate  all  of  the  plant  states  and  run  where 
there  are  more  plant  states  than  the  observer  estimates. 
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Fourth,  results  are  analysed  and  conclusions  drawn  based  on  these  results. 
Reccomendations  for  further  areas  of  research  are  suggested  based  on  the  results  and 
conclusions. 

C.  ORGANIZATION 

The  model  of  th  ■  space  station  is  developed  in  Chapter  II.  The  modal  model  was 
developed  using  modal  analysis  and  discretized  to  form  the  discrete-time  state  equations. 
The  data  for  this  model  was  from  an  early  design  study  by  McDonnell  Douglas 
Astronautics  Company  for  a  dual-keel  space  station.  The  observer  and  its  equations  are 
developed  in  Chapter  III.  Chapter  IV  is  the  simulation  runs  of  the  observer  versus  the 
plant.  Chapter  V  presents  conculsions  and  recommendations  for  further  research. 
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II.  MATHEMATICAL  MODEL 

A.  INTRODUCTION 

Prior  to  the  proposed  space  station  almost  all  of  the  objects  put  into  space  could 
be  treated  as  simple  rigid  bodies  for  the  purpose  of  mathematical  modelling  of  their 
motions.  The  design  constraints  imposed  by  the  high  cost  of  lifting  mass  to  orbit  dic¬ 
tates  a  light,  open  structure  with  considerable  flexing.  Large  space  structures  such  as  the 
space  station,  therefore,  cannot  be  treated  as  rigid  bodies.  The  structure  is  in  fact  lightly 
damped  with  multiple  natural  frequencies.  The  result  is  a  structure  that  will  vibrate  for 
considerable  periods  of  time  whenever  external  forces  are  applied. 

The  space  station  structure  can  be  modeled  as  an  n-DOF  (degree  of  freedom)  system 
consisting  ofn  masses,  springs,  and  dashpots  [Ref.  2:  p.  173-176].  This  straight  forward 
modelling  of  the  coupled  masses  produces  a  system  of  unworkable  complexity.  As  a 
result,  the  system  will  be  modelled  in  terms  of  the  structures  natural  modes  of  vibration. 
The  resulting  system,  while  still  complex,  is  at  least  workable. 

The  model  will  be  developed  in  two  steps.  The  First  will  be  to  generate  the 
continous-time  model  of  the  natural  modes.  The  second  will  yield  the  discrete-time 
model,  developed  from  the  first  model,  for  use  in  the  simulation. 

B.  MODAL  MODEL 

The  space  station  structure  can  be  modeled  as  a  system  of  discrete  masses  coupled 
by  springs  and  dashpots.  The  major  mechanism  of  damping  in  the  structure  is  structural 
damping,  the  internal  dissipation  of  energy  within  the  members,  as  the  structure  vibrates. 
Structural  damping  can  be  shown  to  be  equivalent  to  viscous  damping  and  this  equiv¬ 
alency  is  used  in  the  model  [Ref.  2:  p.  72-73]. 

The  energy  dissipated  by  structural  damping  is: 

Wc/=aX2  (1) 

Wd  =  energy  dissipated  by  structural  damping 
a  =  constant  (force/displacement) 

X  =  displacement 

The  energy  dissipated  by  viscous  damping  is: 

Wv  =  nco)X2  (2) 
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We  can  equate  the  two 


nC'tfX2  =  «X2  (3) 

yielding  an  equivalent  viscous  damping  coefficient: 

C  =  -2-  f4) 

The  second  order  differential  equation  for  a  single  viscously  damped  mass  is: 

mx  +  cx  +  kx  =  F{t )  (5) 


Substituting  Ctq  for  c 


mx  +  '^ox  +  kx=‘F{t)  (6) 

For  multiple  mass  systems  C„  becomes  ~rK  where  (of  is  the  natural  frequency  of  vi- 
bration. 

The  displacement  of  masses  can  be  represented  by  the  second  order  matrix  differ¬ 
ential  equation  [Ref.  3:  p.  3-9], 


Mq(')  +  -gjm)  +  M')  =  m  (7) 

q  =  coordinate  vector 

M  =  system  mass  matrix  (diagonal) 

K  =  equivalent  damping 

IV f 

d  =  damping  coefficient 

(i3f  =  frequency  of  oscilation  of  the  system 

K  =  symmetric  system  stiffness  matrix 

F(/)  =  system  forcing  function 

The  above  equation  represents  a  system  of  second  order  differential  equations  cou¬ 
pled  through  the  stiffness  matrix.  Decoupling  can  be  done  by  expressing  q  in  terms  of 
natural  modes  of  vibration.  The  process  is  called  modal  analysis.  The  independent  dif¬ 
ferential  equations  can  then  be  treated  individually.  The  modal  equations  are  derived 
below. 

First,  the  undamped,  homogeneous  form  of  Eq.  (7) 
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M?(/)  4-  K?(/)  =  0 


(8) 


is  solved.  Let 

q(t)  =  Ax  sin(a>r  +  0)  (9) 

q(t)  =  Axco  cos(a)f  4-  0)  (10) 

q{t)  =  —  Axco2  sin(cot  4-  0)  (11) 

substituting  Eq.  (9)  and  Eq.  (10)  into  Eq.  (11) 

[  —  oj2M  4-  K]A*  sin(cu/  4-  0)  =  0  (12) 

This  equation  has  a  non-trivial  solution  for  all  time  if  and  only  if: 

[K  -  &j2M>  =  0  (13) 

Equation  (12)  has  n  combinations  of  x  (natural  mode  shapes)  and  co  (natural  fre¬ 
quencies)  as  solutions.  These  can  be  grouped  into  matrices: 

X~lX]x2...x„f  (14) 

Q2  =  diagl<ol](o2o2...wln]  (15) 

which  satisfy  the  equation: 

KX  =  Q2MX  (16) 

Several  useful  relations  can  be  derived  from  Eq.  (16).  Premultiplying  Eq.  (16)  by 

XL 

XrKX  =  Q2XrMX  (17) 

The  eigenvectors  can  be  normalized 

XrMX  =  I  (18) 

which  yields 

XrKX  =  Q2  (19) 
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The  equations  of  motion  can  be  uncoupled  through  the  linear  transformation  of  the 
coordinate  system 


n 


q{t)  =  'Yfflit)  =  Xrj{t) 

i=  l 

(20) 

X  - 

n  = 

nit)  = 

modal  matrix 

maximum  number  of  degrees  of  freedom 

transformed  coordinate  vector 

Application  of  the  transformation  to  the  system  Eq.  (7)  yields 

XrM  Xyj(t)  +  XtK  Xi(t)  +  XTKXn(t)  =  XrF(r) 

(21) 

Using  Eq.(lS)  and  Eq.  (19) 

XrM  Xij(t)  =  I  ij[t)  =  r, 

(22) 

XTKX*}{t)  =  tfnit)  =  dQi! 

(23) 

xrK  Xv{t)  =  a2n 

(24) 

therefore 

iH  +  dQ.ii  +  Q2rj  =  XrF  (25) 

Equation  (25)  is  the  modal  model  of  uncoupled  second  order  differential  equations.  The 
motion  of  the  structure  can  be  found  from  the  modal  amplitudes,  rj(t),  using  Eq.  (20). 

C.  DISCRETE-TIME  MODEL 

The  discrete-time  state  space  model  is  found  by  solving  the  continuous-time 
equations.  The  ith  equation  of  motion  is 

nii)  +  d(*>oinit)  +  <ohn(t)  =  X/F  (r)  (26) 

Xf  =  transpose  of  the  ith  mode  shape  vector 
F(r)  =  torquing  force  applied  at  a  point 

The  homogeneous  solution  (F(/)  =  0)  for  Eq.  (26)  is  [Ref.  4:  p.  475-476] 
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7;(0  =  Cxe  yt  cos(wd/)  +  C2e  yt  sin(ov)  (27) 


where 


y  = 


toot 

2 


(28) 


<od 


wo/-y 


2 


(29) 


The  constants  in  Eq.  (27)  can  be  found  by  taking  the  derivative 

7(0  -  (C2<od  -  cxy)e~yt  co s(codt)  -  (C{cod  -  C2y)e~yt  sin (&>/) 
and  evaluating  at  t  =  0 

>7(0)  =  C, 

7(0)  =  C2cod-  C,y 

Solving  for  C,  and  C2 

Ci  -  7(0) 

7(0)  ,  mv 

2  +  (Od 

In  matrix  form 


cr 

1 

0 

'7(0)' 

.Q. 

y 

L  wd 

i 

u<i  J 

.7(0). 

(30) 


(31) 

(32) 

(33) 

(34) 

(35) 


Rewriting  Eq.  (27)  and  Eq.  (30)  in  matrix  form 

[7(0]  J  e~yt  cos(o)dt)  e~yl  sin(a>/)  ]fC,] 

L7(0j  [*_y,0  cos(av)  +  cod  sin(cod/)]  e~yt[cod  cos(codr)  -  y  si n(codt)J  J|_C2J 


Substituting  Eq.  (35)  into  Eq.  (36),  the  solution  can  be  written  in  terms  of  the  initial 
conditions 
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M= 

UwJ 


-yt> 


X  cos(c $4)  +  —  sin(co/)] 
-  737  d~y!  sin(a)/) 


757  e"*"  sin(w/) 
e~y,£  cos (<u/)  -  —  sin(o)/)] 


>7(0) 

>7(0) 


(37) 


Letting 


(38) 


and 


A,  = 


-L.  0-y 


sin(torfr) 


e-yI£  cos i(qdt)  +  —  sin(co/)]  ^ 

~  757  «~v'  sin(av)  e~yt£  cos (corf/)  - ~  sin(<u/)] 


(39) 


the  solution  can  be  written  as 


XX0  =  A/(0X/(O)  (40) 

where  A,  is  the  state  transition  matrix  of  the  ith  mode.  The  non-homogeneous  solution 
is 

X/(0  =  A/r)X/0)  +  Bpr,rF(0)  (41) 

where  the  discrete-time  input  matrix,  for  constant  F,  is  given  by 


B;  =  fW)r5r 
Jo 


(42) 


and  T  =  [0  l]r  is  the  input  matrix  for  the  continuous-time  system,  and  T is  the  sampling 
time.  Solving  Eq.  (42)  yields 


B/  = 


“T  [1  -  e~V  cos(w^)  ~  7J7  e~rt  sin(a)/)] 

-57  e~y>  sin(*V) 


(43) 


The  discrete-time  state  equation  for  the  ith  equation  of  motion  can  be  written  as 

Xi(kT+  1)  =  A((7)X^7)  4-  B,(7>/F(*7)  (44) 

where  A,  and  B,  are  evaluated  at  /  =  T .  Here, 

X,  =  vector  of  the  ith  modal  amplitude  and  the  ith  modal  velocity 
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A,  =  ith  state  transition  matrix 

B,  =  ith  input  vector 

xj  =  transpose  of  the  ith  mode  shape  vector 
F  =  distrubuted  force  on  the  plant 

T  =  sampling  time 

k  =  time  index 

Equation  (44)  can  be  expanded  to  include  the  disturbance  input,  w{kT)  : 

X,{kT+  1)  =  A£T)Xt{kT)  +  B,(7>/[F(*7)  +  w(*7)]  (45) 

Equation  (45)  is  the  discrete-time  mathematical  model  describing  the  motion  of  the 
structure  in  terms  of  its  natural  modes  of  vibration. 


III.  THE  OBSERVER 


A.  INTRODUCTION 

The  observer  design  will  be  required  to  estimate  the  modal  states  in  a  noisy 
enviroment.  Kalman  filtering  is  the  most  widely  used  technique  for  accomplishing  the 
production  of  state  estimates  in  a  noisy  enviroment  [Ref.  5:  p.159].  The  steady  state 
Kalman  filter  was  selected  to  minimize  the  computations  during  the  actual  plant  ob¬ 
server  operation.  Use  of  a  steady-state  gain  matrix  for  the  observer  allows  the  matrix 
to  be  computed  separately  from  the  operational  observer,  reducing  the  computer  power 
required  for  the  observer  and  allowing  the  algorithm  to  operate  more  rapidly. 

B.  KALMAN  FILTER  EQUATIONS 

The  discrete  Kalman  filter  provides  state  estimates  for  the  following  dynamic  sytem 
[Ref.  5:  p.  159-162], 


X(k  +  1)  -  AX(A)  +  BU(A)  +  BnW(A)  (46) 

Y(*+  1)«CX(*+  1)  +  V(*+  1)  (47) 


X 

=  n  x  1 

state  vector 

u 

=  P  x  1 

control  vector 

w 

=  r  x  1 

plant  noise  vector 

Y 

=  m  x  1 

measurement  vector 

V 

=  m  x  1 

measurement  noise  vector 

A 

=  n  x  n 

state  transition  matrix 

B 

=  n  x  p 

control  input  matrix 

Bn 

=  n  x  r 

plant  noise  input  matrix 

C 

=  m  x  n 

measurement  matrix 

The  plant  noise  vector  W (k)  is  gaussian  white  noise  with 


E(W(*)}  =  0 

(48) 

E{W(*)Wr(*)}  =  Q 

(49) 
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for  all  k  =  0,1,2,...  ,  and  Q  is  a  positive  semi-definite  r  x  r  matrix.  V(A)  is  gaussian  white 
noise  with 


E{V(A)}  =  0  (50) 

E{V(*)Vr(*)}  =  R  (51) 

for  all  k  =  0,1,2,... ,  and  R  is  a  positive  definite  m  x  m  matrix.  The  two  random  processes 
W(k)  and  V(£)  are  assumed  to  be  independent,  so  that 

E{V(/)W(/c)}  =  0  (52) 

for  ally  =  1,2,...  ,  and  k  =  0,1,2,...  The  intial  state  X(0)  is  assumed  to  be  a  gaussian  ran¬ 
dom  vector  with 


E{X(0)}  =  0  (53) 

It  is  assumed  that  X(0)  is  independent  of  W(&)  and  V(&). 

The  optimal  estimate  of  X{k  +  1)  is  denoted  X{k  +  1  |  k  +  1).  The  Kalman  filter  is 
designed  to  minimize 

J  =  E{[X(/c  +  1)  -  X(A  +1 1  k  +  l)]r[X(*  +  1)  -  X(/c  4-  I  (ft  +  1)]}  (54) 

The  recursive  realtions  for  generating  X(k  +  1  j  k  +  1)  are 

X(*  +  1  1  k)  =  A X(*  |  k)  +  BU(*)  (55) 

X(k  +  1  |  k  +  1)  =  X{k  +  1 1  *)  +  G(k  +  1)[Y(A  +  1)  -  CX(^  +  1 1  *)]  (56) 

A  A 

for  k  =  0,1,2,...  ,  where  X(0  1 0)  =  0.  X(0  1 0)  is  set  equal  to  zero  since  the  expectation  of 
X(0)  is  zero. 

G{k  +  1)  is  an  n  x  m  matrix  ,  called  the  Kalman  Gain  Matrix  which  is  specified  by 
the  realtions: 


P(H1|A)  =  AP {k  |  k) Ar+  BnQ(/c)Bnr  (57) 

G(k  +  1)  =  P(A  +  1 1  A)Cr[CP(^  +  1 1  k) Cr+  R(k  +  I)]"1  (58) 

P(k  +  1  |  k  +  1)  =  [I  -  G(*  +  l)C]P(A  +  1 1  k)  (59) 
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p [k  |  k )  is  the  covariance  matrix  of  the  error  between  the  states  and  their  estimates 

P (k  |  k)  -  E{[X(A)  -  X(A  |  k)][X(k)  -  X(A  J  A)]7}  (60) 

Since  we  are  using  the  steady  state  gains  the  choice  of  P(0  |  0)  is  irrelevant.  P(0  |  0)  is 
initialized  to  zero  in  the  gain  derivation  program  for  simplicity  [Ref.  6:  p.  139-140]. 

C.  STEADY-STATE  SOLUTION 

If  Equations  (57),  (58),  and  (59)  are  repeatedly  iterated,  G(k  +  1)  will  converge  to  a 
steady  state  value  [Ref.  7:  p.  263]. 


Gw  =  lim  G(k  +  1)  (61) 

k  —*■  oo 

The  values  of  G„  (or  G)  can  be  substituted  into  Eq.  (56)  making  the  steady  state 
Kalman  filter 


X(*  +  1  |  k)  =  AX(*  |  k)  +  BU(A)  (62) 

X(k  +  1  |  k  +  1)  =  X(A  +  1  |  k)  +  G[Y(/c  +  1)  -  CX(k  +  1  1  A)]  (63) 

D.  OBSERVER  PERFORMANCE 

The  performance  of  an  observer  is  judged  by  how  accurately  and  rapidly  it  estimates 
the  desired  states.  The  performance  measure  of  the  observer  as  a  whole  is  shown  in 
equation  (54).  The  normalized  performance  of  the  observer  for  individual  states  is 

J(  =  E[(*,--jq)2]/E[>,2]  (64) 


which  can  be  found  using  Eq.  (65) 

OO  CO 

J(-Yw*)-Wr.+ 

k=0  k=0 

J,  =  performance  measure  for  the  ith  state 
x,{k)  =  value  of  the  iih  state  at  k 

x,{k)  =  observer  estimateof  ith  state  at  k 
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T,  =  sample  interval 

A  normalized  performance  measure  is  used  to  aid  comparison  of  the  performance  of  the 
observer  in  estimating  various  states.  From  Eq.  (65)  it  can  be  shown  that  if  x,(k )  =  0  for 
all  k  —  0,1,2,...  that  J,  would  be  unity.  Therefore,  the  better  the  performance  of  the  ob¬ 
server,  the  smaller  the  fraction  of  one  J,  will  be. 
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IV.  SIMULATION 


A.  INTRODUCTION 

The  objectives  of  the  simulation  were  to 

•  determine  the  sensitivity  of  the  observer  performance  and  settling  time  to  changes 
in  the  ratio  of  plant  noise  to  measurement  noise, 

•  determine  the  effect  on  observer  performance  and  settling  time  of  increasing  the 
number  of  modes  observed  in  the  matched  plant/ observer,  and 

•  determine  the  performance  for  the  reduced  order  observer. 

B.  PLANT  AND  OBSERVER  DATA 

The  dynamic  model  is  a  truncated  form  of  a  preliminary  space  station  configuration; 
the  phase  II  dual  keel  structure,  i  The  full  model  consists  of  an  infinite  number  of  natural 
modes  but  this  was  restricted  to  the  first  ten  active  modes  for  this  study  due  to  limita¬ 
tions  on  computer  resources.  As  will  be  shown  reasonable  data  can  be  obtained  with  this 
simplification  in  examining  the  observer  performance. 

C.  SIMULATION  PROGRAMS 

The  simulation  was  broken  down  into  two  segments  due  to  the  large  memory  and 
computational  time  requirements.  The  first  program  computed  the  steady  state  observer 
gain  matrix  (G).  The  second  program  ran  the  observer  and  the  plant  when  the  plant 
was  subjected  to  an  impulse  excitation. 

The  steady  state  observer  gain  matrix  (G)  was  obtained  by  repeated  iteration  of 
equations  (57),  (58  and  (59).  The  equations  were  were  run  until  the  values  of  the  ma¬ 
trix  changed  by  less  than  a  set  fraction.  The  following  formula  was  used  to  check  the 
changes  in  the  gain  matrix  elements 

4gy  =  [ft/*  +  1)  -  »/*)]/&/*  +  1)  (66) 

The  program  was  terminated  when  <5gv  was  less  than  10-10 . 

The  settling  time  for  the  estimates  of  the  states  to  be  within  2%  of  the  actual  states 
was  determined  by  finding  the  eigenvalues  of  A  -  G*C  then  computing  as  follows  [Ref. 
6:  p.  139-1431 


1  The  model  for  preliminary  station  configuration  was  provided  courtesey  of  McDonnel 
Douglas  Astronautics  Company,  5301  Bolsa  Avenue,  Huntington  Beach,  CA  92647. 


14 


Ts  =  log(.02 )/  log(;MCCmjn) 


(67) 


The  expected  error  in  the  sensor,  i.e.,  the  standard  deviation  of  the  noise  in  the 
measurement,  was  choosen  as  10-3  feet  per  sec.  per  sec.  based  on  the  natural  frequencies 
in  the  structure  and  reasonable  sensor  sensitivity  [Ref.  2:  p.  79-80).  The  expected  plant 
noise  was  varied  to  find  the  range  of  ratios  between  plant  and  measurement  noise  that 
the  filter  would  be  effective.  This  approach  was  taken  since  the  plant  noise  contributors 
are  not  currently  well  defined. 

The  second  program  subjected  the  plant  as  modeled  in  Eq.  (45)  to  an  impulse 
excitation  and  then  had  the  observer  estimate  the  selected  states  using  observer 
equations  (62)  and  (63).  Observer  performance  was  computed  using  Eq.  (65). 

A  third  program  was  used  to  find  the  contribution  of  unobserved  modes  to  the  noise 
in  the  Kalman  observer.  The  program  ran  the  plant  subject  to  an  impulse  excitation  and 
computed  the  product  of  the  measurement  matrix  C  times  the  unobserved  modes  of  the 
state  vector  X{k)  for  a  measure  of  the  noise  contributed  by  the  unobserved  modes. 

The  three  programs  are  listed  in  the  appendices. 

D.  EFFECT  OF  PLANT  TO  MEASUREMENT  NOISE  RATIO  ON  OBSERVER 
PERFORMANCE 

The  ratio  of  the  variance  of  the  plant  noise  (PN)  to  the  variance  of  the  measurement 
noise  (MN)  was  found  to  have  a  strong  effect  on  the  Kalman  Observer  performance  (J) 
and  settling  time  (TJ  .  Figures  1  through  6  show  the  observer  performance  for  a  3  mode 
matched  plant  and  observer  system  for  progressively  smaller  PN/MN  ratios.2  Figure  7 
shows  the  performance  for  the  seventh  mode  (position)  versus  several  values  of  PN  MN. 
Figure  8  is  the  settling  time  versus  the  same  PN/MN  ratios. 

The  figures  show  that,  for  all  of  the  plotted  performance  values,  the  observer  per¬ 
formance  is  at  least  marginally  acceptable  regardless  of  the  PN/MN  ratio.  Decreasing 
the  PN.'MN  ratio  leads  to  an  even  more  rapid  degradation  in  observer  performance. 
The  settling  times  also  rapidly  increase  as  the  PN/MN  ratio  decreases. 


2  Figures  1-6  and  9-15  show  the  performance  measure  for  each  mode.  The  bar  for  the  mode 
position  is  immeadiately  to  the  right  of  the  numbered  tick  mark  on  the  x-axis  scale,  the  mode  ve¬ 
locity  is  next  to  it  immeadiately  adjacent  to  the  tick  mark  without  a  number. 
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E.  EFFECTS  OF  INCREASED  MODES  ON  OBSERVER  PERFORMANCE 

The  matched  plant,  observer  was  run  with  increasing  numbers  of  modes  to  see  if 
there  was  an  effect  on  observer  performance  (J)  or  settling  time  Ts  .  Figures  7  through 
17  are  of  observer  performance  for  systems  with  increasing  numbers  of  modes  in  the 
system  being  observed.  Figure  18  is  of  settling  time  versus  the  number  of  modes  in  the 
system.  The  ratio  of  PN/MN  was  kept  constant  at  PN/MN  =  2.5  x  109. 

The  increasing  of  the  number  of  modes  for  the  matched  plant; observer  had  neglible 
effect  on  the  performance  for  the  individual  modes.  The  performance  value  for  the 
modes  was  effectively  constant.  Settling  times  for  the  observers  increased  as  the  number 
of  modes  was  increased. 

F.  REDUCED  ORDER  KALMAN  OBSERVER 

The  Kalman  Observer  has  been  shown  to  be  effective  where  the  number  of  modes 
observed  matches  the  number  of  modes  in  the  plant.  The  Kalman  Observer  was  then 
run  with  the  one  less  mode  observed  than  the  number  of  modes  in  the  plant.  The  gain 
matrix  (G)  from  the  matched  system  was  used.  The  observer  failed  with  the  state  esti¬ 
mates  produced  by  the  observer  becoming  excessively  large  and  having  settling  times  of 
hours  vice  minutes.  Since  the  purpose  of  the  observer  was  to  provide  estimates  for  use 
in  controlling  the  plant  the  time  delay  makes  the  estimates  unusable. 

The  cause  of  the  observer  failure  is  apparent  when  you  look  at  the  last  portion  of 
Eq.  (56)  of  the  Kalman  Observer 


G[Y(fc  +  1)  -  C \(k  +  1  |  *)]  (68) 

A 

This  portion  of  the  observer  equation  is  the  correction  of  X(k  +  1  |  k)  to  produce 
X(k  -t- 1  |  k  +  1)  .  The  design  of  the  Kalman  observer  is  to  produce  an  estimate  despite 
the  measurement  noise  but,  with  the  reduced  order  filter  there  is  additional  unanticipated 

A 

noise  which  causes  over  correction  of  the  values  of  A'  leading  to  the  state  estimates  being 
excessively  large  and  settling  times  being  too  long.  This  can  be  shown  by  examining 
what  composes  Y(k  +  1)  -  CX{k  +  1  |  k) 
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x2(k) 
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x2(k) 
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T 
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xm{k) 


0 

T 
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o 


(69) 


C  times  the  state  xm^{k)  through  xn(k)  is  unanticipated  noise  so  if 


x\{k) 

*,(*) 

xi(k) 

x2(*) 

T 

-C 

T 

i 

i 

xm-\(k) 

4-i  (k) 

xJk) 

4  (k) 

(70) 


the  remaining  portion  of  the  C  matrix  times  the  modal  states  is  an  equivalent  noise. 

Table  (1)  shows  the  growth  of  the  unanticipated  noise  in  the  filter  as  the  number 
of  unobserved  modes  in  the  plant  grows.  Table  (2)  shows  the  individual  contributions 
of  the  individual  modes  when  left  unobserved.  Table  (I)  shows  that  the  unanticipated 
noise  is  much  larger  than  that  expected  by  the  filter  (ICh3).  Table  (2)  show's  that  there 
are  modes  that  do  not  markedly  contribute  to  the  noise  and  that  they  might  successfully 
be  left  unobserved  if  the  measurement  noise  estimate  was  already  much  larger  than  these 
noise  sources. 
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Table  1.  CUMULATIVE  UNANTICIPATED  NOISE  FROM  UNOBSERVED 


MODES 


Num¬ 
ber  of 
Unob¬ 
served 
Modes 

Unob¬ 

served 

Modes 

El 

E2 

E2 

1 

10 

0.647 

97.440 

3.277 

2 

10-11 

366.354 

95.764 

3.142 

3 

10-12 

366.355 

95.764 

3.142 

4 

10-13 

365.565 

95.855 

3.143 

5 

10-14 

365.426 

96.032 

5.704 

6 

10-15 

144195.7 

116.205 

2201.94 

7 

10-16 

148006.8 

170.142 

5475.21 

8 

10-17 

473974.3 

39424.1 

5692.50 

9 

10- IS 

474344.2 

60078.8 

9419.77 

10 

10-19 

474358.5 

68987.7 

9865.27 

Table  2.  UNANTICIPATED  NOISE  FROM  UNOBSERVED  MODES  BY 


MODE 


Unobserved 

Mode 

El 

E2 

E3 

10 

0.64716 

97.4403 

3.27761 

11 

359.342 

0.20929 

0.21429 

12 

0.9353E-08 

0.1364E-07 

0.2I96E-07 

13 

0.3852E-02 

0.4409E-02 

0.9017E-03 

14 

0.5615E-03 

0.2592E-0I 

2.57554 

15 

143090.9 

17.9682 

2167.02 

16 

195.736 

83.3458 

5675.91 

17 

324471.2 

39252.26 

221.170 

18 

216.240 

21133.6 

3736.93 

19 

7.69298 

8829.95 

458.504 

20 

2.3194 

3949.16 

108.981 

*10 

35.0 


PERFORMANCE  MEASURE  (J)  PN/MN=1.0D12 


Figure  1.  Observer  Performance  (J)  PN/MN  =  1.0dl2 
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PERFORMANCE  MEASURE  (J)  *10 
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Figure  2.  Observer  Performance  (J)  PN/MN  =  l.Odll 
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PERFORMANCE  MEASURE  (J) 


8  PERFORMANCE  MEASURE  (J)  PN/MN=5.0D09 


Figure  4.  Observer  Performance  (J)  PN/MN  =  5.0d09 
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PERFORMANCE  MEASURE  (J) 
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Figure  5.  Observer  Performance  (J)  PN/MN  =  2.5d09 
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Observer  Performance  (J) 
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24 


PERFORMANCE  U)  MODE  7 

0.000  0.004  0.008  0.012  0.016  0.020  0.024 


26 


PERFORMANCE  MEASURE  (J) 
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Figure  9.  Observer  Performance  (J)  4  Modes  (7  -  10) 
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PERFORMANCE  (J)  5  MODES  PN/MN=2.5D09 


Figure  10.  Observer  Performance  (J)  5  Modes  (7  -  11) 
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PERFORMANCE  MEASURE  (J) 


Figure  11.  Observer  Performance  (J)  6  Modes  (7  -  12) 
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PERFORMANCE  (J)  7  MODES  PN/MN=2.5D09 


Figure  12.  Observer  Performance  (J)  7  Modes  (7  -  13) 


PERFO 
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PERFORMRNCE  MEASURE  (J) 


MODE 


Figure  14.  Observer  Performance  (J)  9  Modes  (7  -  15) 
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PERFORMANCE  (J)  10  MODES  PN/MN=2.5D09 


Figure  15.  Observer  Performance  (J)  10  Modes  (7  -  16) 
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SETTLING  TIME  (SEC. 


Figure  16.  Settling  Time  versus  number  of  Modes  Observed 
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V.  CONCLUSIONS  AND  RECOMMENDATIONS 

A.  CONCLUSIONS 

Simulations  runs  showed  that  a  matched  plant/ observer  can  work  if  the  following 
criterions  are  meet: 

•  The  ratio  of  plant  noise  to  measurement  noise  is  sufficiently  high  to  produce  a  us¬ 
able  settling  time. 

•  Sufficient  computational  power  is  available  to  run  the  matched  observer.  The 
amount  of  memory  and  number  of  computation  goes  up  as  the  number  of  modes 
observed  increases. 

Utilizing  a  reduce  order  observer  for  an  arbitrarily  selected  set  of  modes  is  not  fea¬ 
sible.  The  non-observed  modes  add  so  much  noise  to  the  system  that  settling  times  and 
observer  performance  are  so  poor  as  to  render  the  observer-  useless  for  obtaining  state 
values  for  plant  control. 

B.  RECOMENDATIONS 

The  work  on  the  Kalman  Observer  for  Large  Space  Structures  lead  to  the  following 
recommendations  for  further  research: 

•  Identify  those  modes  that  contribute  the  largest  noise  to  the  Kalman  observer  and 
set  the  observer  to  estimate  these  states  in  addition  to  those  required  for  plant 
control.  A  possible  method  for  identifying  the  modes  that  contibute  the  largest 
noise  to  the  observer  would  be  the  Karhunen-Loeve  expansion. 

•  Modifying  the  plant,  observer  to  model  the  use  of  sensors  at  additional  positions 
to  see  if  the  increase  in  the  data  rate  will  help  decrease  settling  time. 

•  Modify  the  model  to  incorporate  noise  injection  into  more  than  one  location  .  The 
current  model  has  noise  injected  at  only  one  position,  a  useful  simplification  for 
initial  analysis  but  not  realistic. 
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APPENDIX  A.  KALMAN  GAIN  MATRIX  GENERATION  PROGRAM 


*****  GGAIN  ***** 
*****  ADAPTED  TO  RUN  KALMAN  FILTER  AND  COMPUTE  THE  ***** 
*****  g  MATRIX  BY  ITERATION  STOPPING  WHEN  THE  'k'tcMrk 
*****  THE  MATRIX  GOES  TO  STEADY  STATE  ***** 


VARIABLE  DECLARATIONS 


***** 


EXTERNAL  STMTRX , DLINRG , EXCMS ,  DEVCRG 
CHARACTER*6  NAM 

CHARACTER* 1  AGAIN, CORECT.RAGAIN 

INTEGER  R0WN1 , R0WN2 ,R0WN3 , COUNT , NODE , MODE , KQ , EMODE , SMODE , R2M , C2M 
INTEGER  CT,CF,KADJ,CFADJ,LOOP,PRNT, JJ, JK,N1 , JR,KR,MR, ISEED,M2 
INTEGER  JL, J1 , JM 

REAL  LAMA(IOO),  UGVEX( 684, 100) ,RN0DE,RM0DE,MIN 
REAL*8  PHI(2,2,100) ,GAMMA( 2,100) ,EGT,GMA,WN,W1 ,X1T,X2T,TIME 
REAL*8  A( 200, 200) , B(200,3) ,F(3,  50) .IMPLSE, ENERGY 
REAL*8  C0SW1T,SINW1T,X1( 100) ,X2( 100) ,COST( 100) 

REAL*8  DAMP , SAMPT , PI , SAMPTM , SUM1 , SUM2 , SUM3 , SUMC 
REAL*8  C(6,200) ,  IDENT(  50,  50),  RMN(6,6),  QPN(3,3) 

REAL*8  PK(  50,  50),  Y(6),  BN(200,3) 

REAL* 8  PNVARX,  PNVARY,  PNVARZ 

REAL*8  MNVX1,  MNVY1 ,  MNVZ1,  SUM,  BQBT(50,50) 

REAL*8  TMP1(  50,3),  TMP2(3,3),  TMP3(  50,  50) 

REAL*8  PK1(  50,  50) ,G(  50,3) 

REAL*8  DY(3) ,  ES,ED,ESUM,CGN,PRT 

REAL*8  SF,  N9 ,  TCHK,  ACHK,  HI,  H2,  H3,  H4,  H5,  H6 

REAL*8  AGC( 100, 100) 

COMPLEX*8  EVAL(IOO),  EVEC  (100,100) 


***** 


VARIABLE  DEFINITIONS 


***** 


STMTRX  =  SUBROUTINE  EXTABLISHES  STATE  TRANSITION  MATRICIES 

LAMA  =  VECTOR  OF  THE  SQUARE  OF  THE  NATURAL  FREQUENCIES 

UGVEX  =  MODE  POSITONS  AND  SLOPES  OF  THE  NODAL  POINTS 

PHI  =  STATE  TRANSITION  MATRICIES  FOR  EACH  MODE 

GAMMA  =  INPUT  TRANSITION  MATRIX 

A  =  DIAGONAL  MATRIX  CONSISTING  OF  PHI 

B  =  INPUT  MATRIX  OF  GAMMA  AND  CONTROL  SLOPES 

DAMP  =  DAMPING  FACTOR 

SAMPT  =  SAMPLING  TIME 


GMA00010 

GMA00020 

GMA00030 

GMA00040 

GMA00050 

GMA00060 

GMA00070 

GMA00080 

GMA00090 

GMA00100 

GMA00110 

GMA00120 

GMA00130 

GMA00140 

GMA00150 

GMA00160 

GMA00170 

GMA00180 

GMA00190 

GMA00200 

GMA00210 

GMA00220 

GMA00230 

GMA00240 

GMA00250 

GMA00260 

GMA00270 

GMA00280 

GMA00290 

GMA00300 

GMA00310 

GMA00320 

GMA00330 

GMA00340 

GMA00350 

GMA00360 

GMA00370 

GMA00380 

GMA00390 

GMA00400 

GMA00410 

GMA00420 

GMA00430 

GMA00440 

GMA00450 

GMA00460 

GMA00470 

GMA00480 

GMA00490 

GMA00500 

GMA00510 


36 


noooooonnonnonnnoociooonnooonoooooooooonoonoononoooonnooo 


TCX,  TCY,  TCZ  =  CONTROL  TORQUE  VALUES 

ENERGY  =  TOTAL  SYSTEM  ENERGY 

IMPLSE  =  IMPULSE  INPUT  FUNCTION 

MIN  =  NUMBER  OF  MINUTES  SYSTEM  WILL  BE  OBSERVED 

SMODE  =  NUMBER  OF  STARTING  MODE  (INT) 

MODE  =  NUMBER  OF  MODES  (INT) 

EMODE  =  NUMBER  OF  THE  LAST  MODE  (INT) 

NODE  =  NUMBER  OF  THE  NOISE  INPUT  MODE  (INT) 

***  NOISE  SLOPE  LOCATIONS  IN  DATA  MATRIX  *** 

ROWN1  =  X-SLOPE  LOCATION 

ROWN2  =  Y-SLOPE  LOCATION 

ROWN3  =  Z-SLOPE  LOCATION 

C  =  OUTPUT  MATRIX  FOR  Y 

IDENT  =  IDENTITY  MATRIX 

RMN  =  MEASUREMENT  NOISE  COVARIANCE  MATRIX 

QPN  =  PLANT  NOISE  COVARIANCE  MATRIX 

PNVARX  =  PLANT  NOISE  X-SLOPE  VARIANCE 

PNVARY  =  PLANT  NOISE  Y-SLOPE  VARIANCE 

PNVARZ  =  PLANT  NOISE  Z-SLOPE  VARIANCE 

MNVARX  =  MEASUREMENT  NOISE  X-SLOPE  VARIANCE 

MNVARY  =  MEASUREMENT  NOISE  Y-SLOPE  VARIANCE 

MNVARZ  =  MEASUREMENT  NOISE  Z-SLOPE  VARIANCE 

I SEED  =  INITIALIZATION  FOR  RANDOM  NUMBER  GENERATOR 

XKAL  =  X  MATRIX 

Y  =  OUTPUT  MATRIX 

RNDM  =  RANDOM  NUMBERS  USED  FOR  WHITE  NOISE  IN  MEASUREMENTS  AND 
IN  PLANT  FORCES 

BN  =  B  MATRIX  TO  MULTIPLY  NOISE  DISTURBANCES 
TNX,TNY ,TNZ=  NOISE  TORQUES  X,Y,Z  SLOPES 
M2=2*MODE 


**********  SAMPLE  OF  SPACE  EXEC  FILE  ************** 

THIS  FILE  MUST  BEGIN  IN  COLUMN  1  AND  RUN  WITH  THE  FOLLOWING 
SEQUENCE  FOR  THE  INITIAL  RUN  OF  THE  PROGRAM: 

FORTVS  SPACE  (COMPILES  PROGRAM) 

SPACE  (EXECUTES  EXEC  FILE) 

LOAD  SPACE  (START  (LOADS  AND  EXECUTES  PROGRAM) 

SUBSEQUENT  PROGRAM  RUNS  CAN  ELIMINATE  "FORTVS  SPACE"  IF  NO 
CHANGES  HAVE  BEEN  MADE  TO  THE  PROGRAM,  AND  CAN  ELIMINATE 
RUNNING  THE  EXEC  FILE. 

FI  4  DISK  THESIS  INPUT  B  (PERM 
FI  8  DISK  UTILITY  DATA  (RECFM  VS  BLOCK  133  PERM 
FI  11  DISK  CNTRL  OUTPUT  (RECFM  F  BLOCK  80  LRECL  80  PERM 
FI  13  DISK  GAMMA  OUTPUT  (RECFM  VS  BLOCK  133  PERM 
FI  14  DISK  MODE  OUTPUT  (RECFM  F  BLOCK  80  LRECL  80  PERM 

FI  16  DISK  COST  OUTPUT  (RECFM  F  BLOCK  80  LRECL  80  PERM 

FI  17  DISK  PRT  OUTPUT  (RECFM  F  BLOCK  80  LRECL  80  PERM 

FI  18  DISK  ERROR  DATA  (RECFM  F  BLOCK  80  LRECL  80  PERM 

FI  19  DISK  END  FILE  (RECFM  F  BLOCK  80  LRECL  80  PERM 
FI  20  DISK  GMAT  FILE  (RECFM  F  BLOCK  80  LRECL  80  PERM 


GMA00520 

GMA00530 

GMA00540 

GMA00550 

GMA00560 

GMA00570 

GMA00580 

GMA00590 

GMA00600 

GMA00610 

GMA00620 

GMA00630 

GMA00640 

GMA00650 

GMA00660 

GMA00670 

GMA00680 

GMA00690 

GMA00700 

GMA00710 

GMA00720 

GMA00730 

GMA00740 

GMA00750 

GMA00760 

GMA00770 

GMA00780 

GMA00790 

GMA00800 

GMA00810 

GMA00820 

GMA00830 

GMA00840 

GMA00850 

GMA00860 

GMA00870 

GMA00880 

GMA00890 

GMA00900 

GMA00910 

GMA00920 

GMA00930 

GMA00940 

GMA00950 

GMA00960 

GMA00970 

GMA00980 

GMA00990 

GMA01000 

GMA01010 

GMA01020 

GMA01030 

GMA01040 

GMA01050 

GMA01060 

GMA01070 
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c 

c 

c 

c 


c 

c 

c 

c 

c 


c 

c 

c 

c 


5 

c 

1001 

1002 

1008 

C 

500 

C 

C 

C 

c 

700 
C 

C 

C 

C 

C 

C 

701 
C 

C 

C 

C 


PARAMETER  ( JR=5243,  KR=5397,  MR=262139) 

MIN  =1200. 0 
WT=1.  0D00 

PI  =  4.  ODO  *  ATAN(l.ODO) 

***************************************************  A******* 

*****  READ  LAMA  AND  UGVEX  MATRICIES  ***** 


CALL  EXCMS  ('CLRSCRN') 

WRITE(6, 1008) 

WRITE( 6,*)  ’  READING  LAMA  AND  UGVEX  MATRICIES' 

WRITE(6,*)  '  ' 

THIS  SECTION  READS  THE  LAMA  VECTOR  AND  THE  UGVEX 
MATRIX  AND  STORES  THEM  IN  MEMORY  FOR  FURTHER  RECALL  OF 
DESIRED  LOCATION  DATA. 

READ(4, 1001)  NAM 

READ( 4 , 1002) ( LAMA( I ) , 1=1 , 100) 

READ(4, 1001)  NAM 
DO  5  J  =  1,100 

RE AD (4 , 1002 ) (UGVEX( I , J) , 1=1 , 684 ) 

CONTINUE 

FORMATC IX, A6) 

FORM AT ( IX, 8E15.  8) 

FORMAT( IX,////) 

CALL  EXCMS  (’CLRSCRN’) 

************  STARTING  MODE  NUMBER  ************** 

**  SMODE  7  TO  100  (INTEGER)  **** 

SM0DE=10 

WRITE  (16,700)  SMODE 

FORMAT  ( '  ' , 'STARTING  MODE  NUMBER:  ' ,12) 

***************  NUMBER  OF  MODES  TO  SCAN  ************* 

**  MODE  1  TO  93  (INTEGER) 

MODE=  3 

EMODE  =  SMODE  +  MODE  -  1 
WRITE  (16,701)  MODE 

FORMAT  ('  ',' NUMBER  OF  MODES  SCANNED:  ’,12) 

************  NOISE  INPUT  POSITION  ***************** 

**  NODE  1  TO  114  (INTEGER)  (IF  0  THEN  NO  NOISE  INPUT) 

NODE=  8 


GMA01080 

GMA01090 

GMA01100 

GMA01110 

GMA01120 

GMA01130 

GMA01140 

GMA01150 

GMA01160 

GMA01170 

GMA01180 

GMA01190 

GMA01200 

GMA01210 

GMA01220 

GMA01230 

GMA01240 

GMA01250 

GMA01260 

GMA01270 

GMA01280 

GMAG1290 

GMA01300 

GMA01310 

GMA01320 

GMA01330 

GMA01340 

GMA01350 

GMA01360 

GMA01370 

GMA01380 

GMA01390 

GMA01400 

GMA01410 

GMA01420 

GMA01430 

GMA01440 

GMA01450 

GMA01460 

GMA01470 

GMA01480 

GMA01490 

GMA01500 

GMA01510 

GMA01520 

GMA01530 

GMA01540 

GMA01550 

GMA01560 

GMA01570 

GMA01580 

GMA01590 

GMA01600 

GMA01610 

GMA01620 
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702 

C 

C 

C 

C 


C 

900 

C 

703 
C 

C 

C 

C 

704 
C 

C 

C 

C 

C 

c 


c 

c 

c 

c 

c 


c 

510 


C 

C 

c 


c 


WRITE  (16,702)  NODE 

FORMAT  ('  NOISE  NODE  LOCATION:  ' , 15) 


*************  SAMPLING  TIME  ***• 

**  SAMPT  MUST  BE  LESS  THAN  OR  EQUAL  TO  SAMPTM  ** 
SAMPT  =  .  05 

SAMPTM  =  ((2.  ODO*PI)/SQRT(LAMA(EMODE)))/2. ODO 
IF  ( SAMPT. GE. SAMPTM)  THEN 
SAMPT=SAMPTM 

END  IF 

WRITE  (16,900)  MIN 

FORMAT  ('  ' ,2X,'MIN:  ' ,F8. 3) 

WRITE  (16,703)  SAMPT 

FORMAT  ('  ' .'SAMPLING  TIME:  ’.D12.4) 

***************  DAMPING  FACTOR  *** 

**  DAMP  0.0  TO  1.0  (REAL*8) 

DAMP=.  01 

WRITE  (16,704)  DAMP 

FORMAT  (’  ’, ’DAMPING  FACTOR:  ’ ,D12. 4) 


***  PLANT  NOISE  VARIANCE  *** 

**  PNVARX,  PNVARY ,  PNVARZ  GT  0.  0 

SF1=2. 5D06 

PNVARX=1.  0D00*SF1 
PNVARY=1, 0D00*SF1 
PNVARZ=1.  0D00*SF1 


***  MEASUREMENT  NOISE  VARIANCE  *** 

**  MNVARX,  MNVARY,  MNVARZ  GT  0.  0 
SF=1.  0 

MNVX1=5.  5D-03*SF 
MNVY1=5.  5D-03*SF 
MNVZ1=5.  5D-03*SF 

CALL  EXCMS  ('CLRSCRN') 

WRITE  (6,1008) 

WRITE  (6,*)  '  PROGRAM  RUNNING' 

***********  NOISE  INPUT  LOCATION  *********** 


R0WN3  =  NODE* 6 
R0WN2  =  (N0DE*6)  -  1 
R0WN1  =  (N0DE*6)  -  2 
COUNT  =  0 


GMA01630 

GMA01640 

GMA01650 

GMA01660 

GMA01670 

GMA01680 

GMA01690 

GMA01700 

GMA01710 

GMA01720 

GMA01730 

GMA01740 

GMA01750 

GMA01760 

GMA01770 

GMA01780 

GMA01790 

GMA01800 

GMA01810 

GMA01820 

GMA01830 

GMA01840 

GMA01850 

GMA01860 

GMA01870 

GMA01880 

GMA01890 

GMA01900 

GMA01910 

GMA01920 

GMA01930 

GMA01940 

GMA01950 

GMA01960 

GMA01970 

GMA01980 

GMA01990 

GMA02000 

GMA02010 

GMA02020 

GMA02030 

GMA02040 

GMA02050 

GMA02060 

GMA02070 

GMA02080 

GMA02090 

GMA02100 

GMA02110 

GMA02120 

GMA02130 

GMA02140 

GMA02150 

GMA02160 

GMA02170 
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C 

C 


45 

40 

C 


46 

47 
C 


48 

C 

C 

C 


58 

60 

C 


C 

9999 

C 

C 

C 

C 


C 

C 

C 


90 

92 

94 


*********** 


DO  40  I  =  1,3 
DO  45  J  =  1,3 
RMN( I , J)=0. 0 
CONTINUE 
CONTINUE 

DO  47  1=1,50 
DO  46  J=l,50 
IDENT( I , J)=0.  0 
PK( I , J)=0.  0 
CONTINUE 
CONTINUE 

DO  48  K=1 ,50 

IDENT(K,K)=1.  0 
CONTINUE 

***  INITIALIZE  RMN  AND  QPN  MATRICES  *** 

DO  60  1=1,3 
DO  58  J=1 , 3 
QPN( I , J)=0.  0 
CONTINUE 
CONTINUE 

RMN(1,1)=MNVX1**2 
RMN(2,2)=MNVY1**2 
RMN( 3,3) =MNVZ 1**2 
QPN( 1 , 1 )=PNVARX**2.  0 
QPN( 2 , 2)=PNVARY**2. 0 
QPN( 3,3) =PNVARZ**2 . 0 

FORMAT  ( ’  ' , '  ' ) 

*********************************************************** 

*****  BEGIN  MAIN  PROGRAM  ***** 

*********************************************************** 


CALL  STMTRXC EMODE , SMODE , SAMPT .DAMP , PHI , GAMMA , A , B , LAMA , UGVEX , C , 
+  ROWN 1 , ROWN2 , R0WN3 , BN ) 


***  PRE-LOOP  PORTION  OF  KALMAN  FILTER 
JK=SMODE*2-2 
M2=2*MODE 
DO  94  1=1,3 
DO  92  J=1,M2 
JL=JK+J 
SUM=0. 0 

DO  90  K=l,3 

SUM=SUM+QPN(I,K)*BN(JL,K) 
CONTINUE 
TMP1( J, I)=SUM 
CONTINUE 
CONTINUE 


GMA02180 

GMA02190 

GMA02200 

GMA02210 

GMA02220 

GMA02230 

GMA02240 

GMA02250 

GMA02260 

GMA02270 

GMA02280 

GMA02290 

GMA02300 

GMA02310 

GMA02320 

GMA02330 

GMA02340 

GMA02350 

GMA02360 

GMA02370 

GMA02380 

GMA02390 

GMA02400 

GMA02410 

GMA02420 

GMA02430 

GMA02440 

GMA02450 

GMA02460 

GMA02470 

GMA02480 

GMA02490 

GMA02500 

GMA02510 

GMA02520 

GMA02530 

GMA02540 

GMA02550 

GMA02560 

GMA02570 

GMA02580 

GMA02590 

GMA02600 

GMA02610 

GMA02620 

GMA02630 

GMA02640 

GMA02650 

GMA02660 

GMA02670 

GMA02680 

GMA02690 

GMA02700 

GMA02710 

GMA02720 

GMA02730 


c 

GMA02740 

c 

GMA02750 

DO  98  1=1 ,M2 

GMA02760 

JL=JK+I 

GMA02770 

DO  97  J=1 ,M2 

GMA02780 

SUM=0.  0 

GMA02790 

DO  96  K=1 ,3 

GMA02800 

SUM=SUM+BN( JL , K)*TMP1 ( J , K) 

GMA02810 

96 

CONTINUE 

GMA02820 

BQBT( I , J)=SUM 

GMA02830 

97 

CONTINUE 

GMA02840 

98 

CONTINUE 

GMA02850 

C 

GMA02860 

M2=2*M0DE 

GMA02870 

DO  100  1=1, M2 

GMA02880 

DO  99  J=1 ,M2 

GMA02890 

TMP3( I , J)=0.  0 

GMA029.00 

99 

CONTINUE 

GMA02910 

100 

CONTINUE 

GMA02920 

JL=JK+M2 

GMA02930 

DO  9375  1=1,3 

GMA02940 

DO  9374  J=l, JL 

GMA02950 

C( I , J)=C( I , J)*SF 

GMA02960 

9374 

CONTINUE 

GMA02970 

9375 

CONTINUE 

GMA02980 

C 

GMA02990 

C 

********************************************************** 

GMA03000 

C 

*****  THIS  SECTION  COMPUTES  THE  STATE  UPDATE  ***** 

GMA03010 

C 

********************************************************** 

GMA03020 

ESUM=0. 0 

GMA03030 

COUNT  =  0 

GMA03040 

ENERGY  =  0. 0D0 

GMA03050 

TIME  =  0. 0 

GMA03060 

CGN=0.  0 

GMA03070 

C 

*****  SETS  LOOP  FOR  THE  ITERATIONS  NECESSARY  TO  OBSERVE  ***** 

GMA03080 

C 

*****  THE  SYSTEM  FOR  THE  NUMBER  OF  MINUTES  SPECIFIED  ***** 

GMA03090 

C 

GMA03100 

LOOP  =  INT((MIN*60.  0)/SAMPT) 

GMA03110 

PRT=(DBLE( LOOP))/ 1200.  0 

GMA03120 

PRTA=(DBLE(LOOP) )/2400.  0 

GMA03130 

CNTA=0.  0 

GMA03140 

ACHK=1.  OD-IO 

GMA03150 

H1=0. 0 

GMA03160 

H2=0. 0 

GMA03170 

H3=0. 0 

GMA03180 

H4=0.  0 

GMA03190 

H5=0.  0 

GMA03200 

H6=0. 0 

GMA03210 

TCHK=MIN*60.  0 

GMA03220 

9991 

CONTINUE 

GMA03230 

C 

GMA03240 

TIME  =  TIME+  SAMPT 

GMA03250 

C 

GMA03260 

CGN=CGN+1. 0 

GMA03270 

C 

CNTA=CNTA+1.  0 

GMA03280 

c 

***  START  OF  KALMAN  FILTER  *** 

GMA03290 

c 

GMA03300 

M2=2*M0DE 

GMA03310 

c 

GMA03320 

c 

***  COMPUTATION  OF  PK*AT  *** 

GMA03330 

c 

GMA03340 

JK=2*SM0DE-2 

GMA03350 

DO  175  1=1, M2 

GMA03360 

DO  170  J=1,M2 

GMA03370 

JL=JK+J 

GMA03380 

SUM=0 . 0 

GMA03390 

DO  165  K=1,M2 

GMA03400 

JM=JK+K 

GMA03410 

SUM  =SUM+PK( I ,K)*A( JL, JM) 

GMA03420 

165 

CONTINUE 

GMA03430 

TMP3( I , J)=SUM 

GMA03440 

170 

CONTINUE 

GMA03450 

175 

CONTINUE 

GMA03460 

C 

GMA03470 

C 

GMA03480 

C 

***  COMPUTATION  OF  A*(PK*AT)+  BQBT  =  PK1  *** 

GMA03490 

C 

GMA03500 

DO  190  1=1, M2 

GMA03510 

JL=JK+I 

GMA03520 

DO  185  J=1 ,M2 

GMA03530 

SUM=0. 0 

GMA03540 

DO  180  K=1 ,M2 

GMA03550 

JM=JK+K 

GMA03560 

SUM=SUM+A(JL,JM)*TMP3(K,J) 

GMA03570 

180 

CONTINUE 

GMA03580 

PK1( I , J)=SUM+BQBT( I , J) 

GMA03590 

185 

CONTINUE 

GMA03600 

190 

CONTINUE 

GMA03610 

C 

****  ******* 

GMA03620 

C 

GMA03630 

C 

***  COMPUTE  PK1*CT  **** 

GMA03640 

C 

GMA03650 

DO  205  1=1, M2 

GMA03660 

DO  200  J=1 , 3 

GMA03670 

SUM=0. 0 

GMA03680 

DO  195  K=1 ,M2 

GMA03690 

JM=JK+K 

GMA03700 

SUM=SUM+PK 1 ( I , K ) *C ( J , JM ) 

GMA03710 

195 

CONTINUE 

GMA03720 

TMP1( I , J)=SUM 

GMA03730 

200 

CONTINUE 

GMA03740 

205 

CONTINUE 

GMA03750 

C 

TnnTTflTTflafWTfTfTfTf 

GMA03760 

C 

GMA03770 

C 

***  COMPUTE  C*(PK1*CT)+RMN  *** 

GMA03780 

DO  220  1=1,3 

GMA03790 

DO  215  J=1 ,3 

GMA03800 

SUM=0. 0 

GMA03810 

DO  210  K=1 ,M2 

GMA03820 

JM=JK+K 

GMA03830 

42 


SUM=SUM+C ( I , JM ) *TMP 1 ( K , J ) 

GMA03840 

210 

CONTINUE 

GMA03850 

TMP2( I , J)=SUM+RMN( I ,  J) 

GMA03860 

215 

CONTINUE 

GMA03870 

220 

CONTINUE 

GMA03880 

C 

GMA03890 

C 

***  COMPUTATION  OF  THE  INVERSE  OF  C*PK1*CT  +  R 

GMA03900 

C 

GMA03910 

C 

GMA03920 

CALL  DLINRG  (  3,TMP2,3,TMP2,3) 

GMA03930 

C 

GMA03940 

C 

***  COMPUTE  CT* I NV( C*PK 1*CT+R ) 

GMA03950 

C 

GMA03960 

DO  245  1=1, M2 

GMA03970 

JL=JK+I 

GMA03980 

DO  240  J=l,3 

GMA03990 

SUM=0.  0 

GMA04000 

DO  235  K=1 , 3 

GMA04010 

SUM=SUM+C ( K , JL ) *TMP  2 ( K , J ) 

GMA04020 

235 

CONTINUE 

GMA04030 

TMP1( I , J)=SUM 

GMA04040 

240 

CONTINUE 

GMA04050 

245 

CONTINUE 

GMA04060 

C 

************* 

GMA04070 

C 

GMA04080 

C 

***  COMPUTE  PK1*C*INV(C*PK1*CT+R)  =  G  ***** 

GMA04090 

C 

GMA04100 

DO  260  1=1, M2 

GMA04110 

DO  255  J=1 ,3 

GMA04120 

SUM=0. 0 

GMA04130 

DO  250  K=1 ,M2 

GMA04140 

SUM=SUM+PK1( I ,K)*TMP1(K, J) 

GMA04150 

250 

CONTINUE 

GMA04160 

G( I , J)=SUM 

GMA04170 

255 

CONTINUE 

GMA04180 

260 

CONTINUE 

GMA04190 

C 

GMA04200 

N9=DABS( (G( 1 , 1) -Hl)/G( 1,1)) 

GMA04210 

IF  (N9.GT.ACHK)  THEN 

GMA04220 

GO  TO  7377 

GMA04230 

END  IF 

GMA04240 

N9=DABS((G( 1,3)-H2)/G(1,3)) 

GMA04250 

IF  (N9.GT. ACHK)THEN 

GMA04260 

GO  TO  7377 

GMA04270 

END  IF 

GMA04280 

N9=DABS( ( G( 2 , 1 ) -H3) /G(  2 , 1 ) ) 

GMA04290 

IF  (N9.GT. ACHK)  THEN 

GMA04300 

GO  TO  7377 

GMA04310 

END  IF 

GMA04320 

N9=DABS( (G( 2 , 3) -H4)/G( 2,3)) 

GMA04330 

IF  (N9.GT.ACHK)  THEN 

GMA04340 

GO  TO  7377 

GMA04350 

END  IF 

GMA04360 

N9=DABS( (G( 3 , 3) -H5)/G( 3,3)) 

GMA04370 

IF  (N9.GT. ACHK)  THEN 

GMA04380 

GO  TO  7377 

GMA04390 
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END  IF 

GMA04400 

N9=DABS( (G(M2 , 3) -H6)/G(M2 ,3) ) 

GMA04410 

IF  (N9.GT.  ACHK)  THEN 

GMA04420 

GO  TO  7377 

GMA04430 

END  IF 

GMA04440 

GO  TO  400 

GMA04450 

c 

GMA04460 

c 

GMA04470 

7377 

CONTPIUE 

GMA04480 

H1=G( 1 , 1) 

GMA04490 

H2=G( 1,3) 

GMA04500 

H3=G( 2,1) 

GMA04510 

H4=G(2,3) 

GMA04520 

H5=G( 3,3) 

GMA04530 

H6=G(M2,3) 

GMA04540 

GMA04550 

GMA04560 

GMA04570 

IF  (TCHK. LE. TIME)  THEN 

GMA04580 

GO  TO  400 

GMA04590 

END  IF 

GMA04600 

IF  (CGN. GE.PRT)  THEN 

GMA04610 

C 

GMA04620 

WRITE  (6,*)  ' TIME=  TIME,  ’  SEC.’ 

GMA04630 

C 

GMA04640 

WRITE  (6,*)  1 N9=  ’ ,  N9 

GMA04650 

CGN=0. 0 

GMA04660 

END  IF 

GMA04670 

C 

GMA046d0 

C 

***  COMPUTE  IDENT  -  G*C 

GMA04690 

C 

GMA04700 

DO  275  1=1, M2 

GMA04710 

DO  270  J=1 ,M2 

GMA04720 

JL=JK+J 

GMA04730 

SUM=0. 0 

GMA04740 

DO  265  K=1 , 3 

GMA04750 

SUM=SUM+G( I ,K)*C(K, JL) 

GMA04760 

265 

CONTINUE 

GMA04770 

TMP3( I , J)=  IDENT( I , J) -SUM 

GMA04780 

270 

CONTINUE 

GMA04790 

275 

CONTINUE 

GMAC4800 

C 

******************** 

GMA04810 

C 

GMA04820 

C 

***  COMPUTE  PK=  (INDENT  -  G*C)*PK1 

GMA04830 

C 

GMA04840 

DO  290  1=1, M2 

GMA04850 

DO  285  J=1 ,M2 

GMA04860 

SUM=0. 0 

GMA04870 

DO  280  K=1 ,M2 

GMA04880 

SUM=SUM+TMP3(I,K)*PK1(K,J) 

GMA04890 

280 

CONTINUE 

GMA04900 

PK( I , J)=SUM 

GMA04910 

285 

CONTINUE 

GMA04920 

290 

CONTINUE 

GMA04930 

C 

******************** 

GMA04940 

C 

GMA04950 
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C  END  OF  KALMAN  FILTER  PART  1  -  START  OF  PART  2  **** 

C 

C 

C 

GO  TO  9991 
C 

400  CONTINUE 
C 

WRITE  (20,1008) 

WRITE  (20,*)  'TIME*  '.TIME 
DO  384  1=1, M2 

WRITE  (20,5350)  G( I , 1) ,G( I ,2) ,G( I ,3) 

384  CONTINUE 

5350  FORMAT  ('  ' ,5X,D15. 8  ,5X,D15. 8  ,5X,D15.8  ) 

WRITE  (20,*)  1 N9=  ' ,N9 
C 

C  ***  COMPUTE  AGC  =  A  -  G*C 
C 

M2=2*M0DE 

JK=2*SM0DE-2 

C 

DO  7155  1=1, M2 
JL=JK+I 

DO  7154  J=1 ,M2 
JM=JK+J 
SUM=0. 0 

DO  7153  K=1 ,3 
SUM=SUM+G( I ,K)*C(K, JM) 

7153  CONTINUE 

AGC( I , J)=A( JL, JM) -SUM 

7154  CONTINUE 

7155  CONTINUE 
C 

C 

C 

C  ***  COMPUTE  THE  EIGENVALUES  OF  AGC 
C 

CALL  DEVCRG  (M2,  AGC,  100,  EVAL,  EVEC,  100) 

C 

C  ****  PRINT  EVAL  (EIGENVALUE)  MATRIX 
C 

DO  7157  1=1, M2 

WRITE  (20,*)  '1=  ' ,  I,  ' EIG=  ',  EVAL(I) 

7157  CONTINUE 
C 
C 
C 

599  STOP 
END 
C 
C 

c 

c 

c 

C  THIS  SUBROUTINE  COMPUTES  THE  STATE  TRANSITION  MATRIX  FOR  EACH 


GMA04960 
GMA04970 
GMA04980 
GMA04990 
GMA05000 
GMA05010 
GMA0502 0 
GMA05030 
GMA05040 
GMA05050 
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GMA05290 
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GMA05350 
GMA05360 
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noon  oonno  oonono  noono  ooooo  n  ooo 


OF  THE  100  MODES 


SUBROUTINE  STMTRXC EMODE , SMODE ,T , D , PHI , GAMMA , A , B , LAMA , UGVEX , C , 
+  ROWN 1 , R0WN2 , R0WN3 , BN) 

REAL* 8  WN,GMA,PHI(2,2,100) ,GAMMA(2,100) ,EGT,T,C0SW1T,SINW1T 
REAL* 8  W1 ,D,A( 200,200) ,B( 200 ,3) ,C( 6,200) ,BN(200,3) 

REAL  LAMA(100),UGVEX(684,100) 

INTEGER  SMODE , R , EMODE , J J , KK , ROWN 1 , R0WN2 , ROWN 3 


DO  600  I  =  1  ,100 

WN  =  DBLE(SQRT(LAMA( I))) 

GMA  =  D*WN/2. 0 
EGT  =  DEXP( -GMA*T) 

W1  =  DSQRT( ( WN**2 ) - ( GMA**2 ) ) 
C0SW1T  =  DC0S(W1*T) 

SINW1T  =  DSIN(W1*T) 


IF(WN.  EQ.  0)THEN 

PHI (1,1,1)  =  EGT*C0SW1T 

PHI( 1,2,1)  =  T 

PHI (2, 1,1)  =  0 

PHI (2, 2, I)  =  EGT*C0SW1T 


GAMMA( 1,1)  =  0 
GAMMA( 2,1)  =  0 

ELSE 


PHI( 1,1,1) 
PHI( 1,2,1) 
PHI( 2,1,1) 
PHI( 2 ,2,1) 


EGT*(C0SW1T  +  (GMA*(W1**(-1)))*SINW1T) 
(Wl**( -1) )*EGT*SINW1T 
-(WN**2)*(W1**( -1) )*EGT*SINW1T 
EGT*(C0SW1T  -  (GMA*(W1**( -1) ) )*SINW1T) 


GAMMA(1,I)=(WN^(-2))*(1.D0-EGT*C0SW1T-EGT*(GMA/W1)*SINW1T) 


GMA05520 

GMA05530 

GMA05540 

GMA05550 

GMA05560 

GMA05570 

GMA05580 

GMA05590 

GMA05600 

GMA05610 

GMA05620 
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GMA05660 
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GMA05690 

GMA05700 

GMA05710 

GMA05720 

GMA05730 

GMA05740 

GMA05750 

GMA05760 

GMA05770 

GMA05780 

GMA05790 

GMA05800 

GMA05810 

GMA05820 

GMA05830 

GMA05840 

GMA05850 

GMA05860 

GMA05870 

GMA05880 

GMA05890 

GMA05900 

GMA05910 

GMA05920 

GMA05930 

GMA05940 

GMA05950 

GMA05960 

GMA05970 
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GMA06010 

GMA06020 

GMA06030 

GMA06040 

GMA06050 

GMA06060 
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GAMMAC 2, I)  =  CW1**(-1))*EGT*SINW1T 


ENDIF 


00  CONTINUE 


R  =  1 

DO  610  K  =  1  ,100 


A(R,R)  =  PHI(1,1,K) 
A(R,R+1)  =  PHI( 1 , 2 ,K) 
A(R+1,R)  =  PHI( 2 , 1 ,K) 
A(R+1 ,R+1)  =  PHI(2,2,K) 


***  B  MATRIX  FOR  MULTIPLYING  CONTROL  TORQUES 

B(R, 1)  =  GAMMAC 1,K)*DBLEC UGVEXC 412, K)) 
B(R, 2)  =  GAMMA( 1,K)*DBLE(UGVEX(413,K)) 
B(R,3)  =  GAMMA( 1 ,K)*DBLEC UGVEXC 414, K)) 
BCR+1,1)  =  GAMMA( 2 ,K)*DBLE(UGVEX(412,K)) 
B(R+1 ,2)  =  GAMMAC  2 , K )*DBLE ( UGVEX( 4 13 , K ) ) 
B(R+1 ,3)  =  GAMMAC 2, K)*DBLE(UGVEX(414,K)) 


***  BN  MATRIX  FOR  MULTIPLYING  THE  NOISE  DISTURBANCES 


BNCR, 1)=GAMMA( 1 ,K)*DBLE(UGVEX(R0WN1 ,K) ) 
BN(  R ,  2  )=GAMMA(  1 ,  K)*DBLE(  UGVEXC  ROVTN2  ,K) ) 
BN( R , 3 )=GAMMA( 1 , K)*DBLE( UGVEXC  ROWN3 , K) ) 
BN(R+1, 1)=GAMMA( 2 ,K)*DBLE( UGVEXC R0WN1 ,K)) 
BNC R+ 1 , 2 ) =GAMMAC 2 , K ) *DBLE C UGVEXC R0WN2 , K ) ) 
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GMA06080 
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BN( R+l , 3 )=GAMMA( 2 , K) *DBLE( UGVEX( R0WN3 ,  K)  ) 

GMA06630 

GMA06640 

GMA06650 

GMA06660 

GMA06670 

GMA06680 

GMA06690 

R  =  R+2 

GMA06700 

CONTINUE 

GMA06710 

GMA06720 

GMA06730 

GMA06740 

GMA06750 

GMA06760 

GMA06770 

GMA06780 

***  C  MATRIX  PRODUCTION  *** 

GMA06790 

GMA06800 

GMA06810 

GMA06820 

JJ=-1 

GMA06830 

DO  640  1=1,100 

GMA06840 

JJ=JJ+1 

GMA06850 

KK=I+JJ 

GMA06860 

GMA06870 

GMA06880 

C( 1 ,KK)  =  DBLE(UGVEX(418 , I) ) 

GMA06890 

C( 2 ,KK)  =  DBLE(UGVEX(419 , I) ) 

GMA06900 

C(3,KK)  =  DBLE(UGVEX( 420 , I ) ) 

GMA06910 

GMA06920 

GMA06930 

GMA06940 

KK=KK+1 

GMA06950 

GMA06960 

C(1,KK)=0. 0 

GMA06970 

C(2,KK)=0.  0 

GMA06980 

C(3,KK)=0.  0 

GMA06990 

GMA07000 

CONTINUE 

GMA07010 

GMA07020 

GMA07030 

GMA07040 

RETURN 

GMA07050 

END 

GMA07060 

* 
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APPENDIX  B.  KALMAN  OBSERVER  AND  PLANT  SIMULATION 


*****  SIMRUN  ***** 
*****  ADAPTED  TO  READ  KALMAN  FILETER  G  MATRICE  •fckjcfcic 
*****  THEN  RUN  ALL  N  MODES  OF  THE  PLANT  WHILE  ***** 
*****  USING  A  KALMAN  FILTER  TO  OBSERVE  M  ***** 
*****  number  OF  STATES  ***** 


*********************************************************** 
*****  VARIABLE  DECLARATIONS  ***** 


EXTERNAL  STMTRX.EXCMS 
CHARACTER*6  NAM 

CHARACTER* 1  AGAIN, CORECT,RAGAIN 

INTEGER  ROWN 1 , R0WN2 , R0WN3 , COUNT , NODE , MODE , KQ , EMODE , SMODE , R2M , C 2M 
INTEGER  CT , CF , KAD J , CFADJ , LOOP , PRNT ,JJ,JK,N1,JR,KR,MR,I SEED , M2 
INTEGER  ITYPE(200),  IPVT(IOO),  NS,  NF,  SN,  FN 
INTEGER  JL, Jl, JM  ,  JP,  JQ,  KA,  KB,  KC,  KD,  KE,  KF,  KG 


REAL  LAMA(IOO),  UGVEX( 684,100) ,RNODE ,RM0DE ,MIN 

REAL*8  PHI (2, 2, 100) ,GAMMA( 2, 100) ,EGT,GMA,WN ,W1 ,X1T,X2T,TIME 

REAL* 8  A( 200,200) ,B(200,3),F(3,  50) .IMPLSE, ENERGY 

REAL* 8  C0SW1T,SINW1T,X( 200) 

REAL* 8  TCX ,TCY ,TCZ ,DAMP , SAMPT ,PI , SAMPTM , SUM1 , SUM2 , SUM3 , SUMC 
REAL*8  C(3,200) ,  IDENT(  50,  50),  RMN(3,3),  QPN(3,3) 

REAL*8  Y( 3)  ,  5N(200,3) 

REAL*8  PNVARX,  PNVARY ,  FNVARZ 
REAL*8  MNVARX,  MNVARY ,  MNVARZ 
REAL* 8  SUM,  RNDM(6) ,  RND1 ,  RND2 
REAL*8  XH(  50)  ,BQBT(  50,  50) 

REAL*8  SF1 

REAL*8  TMP1(  50,3),  TMP2(3,3),  TMP3(  50,  50) 

REAL* 8  G(  50,3) 

REAL* 8  XH1(  50)  ,DY(3)  ,  ES ,ED,ESUM,CGN,PRT 

REAL* 8  WT  ,  WD(3) ,  BNWD(200) 

REAL* 8  AX(200)  ,  V(3),  SF  ,  TO,  CTT,  ESS 

REAL* 8  CTG,  XDEL,  E2(100),  XDEL1 ,  ERS,  PRT1,  E3(100),  XS(IOO) 


***** 


VARIABLE  DEFINITIONS 


***** 


STMTRX  =  SUBROUTINE  EXTABLISHES  STATE  TRANSITION  MATRICIES 
LAMA  =  VECTOR  OF  THE  SQUARE  OF  THE  NATURAL  FREQUENCIES 
UGVEX  =  MODE  POSITONS  AND  SLOPES  OF  THE  NODAL  POINTS 
PHI  =  STATE  TRANSITION  MATRICIES  FOR  EACH  MODE 


SIM00010 

SIM00020 

SIM00030 

SIM00040 

SIM00050 

SIM00060 

SIM00070 

SIM00080 

SIM00090 

SIM00100 

SIM00110 

SIM00120 

SIM00130 

SIM00140 

SIM00150 

SIM00160 

SIM00170 

SIM00180 

SIM00190 

SIM00200 

SIM00210 

SIM00220 

SIM00230 

SIM00240 

SIM00250 

SIM00260 

SIM00270 

SIM00280 

SIM00290 

SIM00300 

SIM00310 

SIM00320 

SIM00330 

SIM00340 

SIM00350 

SIM00360 

SIM00370 

SIM00380 

SIM00390 

SIM00400 

SIM00410 

SIM00420 

SIM00430 

SIM00440 

SIM00450 

SIM00460 

SIM00470 

SIM00480 

SIM00490 

SIM00500 

SIM00510 
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GAMMA  =  INPUT  TRANSITION  MATRIX 

A  =  DIAGONAL  MATRIX  CONSISTING  OF  PHI 

B  =  INPUT  MATRIX  OF  GAMMA  AND  CONTROL  SLOPES 

DAMP  =  DAMPING  FACTOR 

SAMPT  =  SAMPLING  TIME 

TCX,  TCY,  TCZ  =  CONTROL  TORQUE  VALUES 

ENERGY  =  TOTAL  SYSTEM  ENERGY 

IMPLSE  =  IMPULSE  INPUT  FUNCTION 

MIN  =  NUMBER  OF  MINUTES  SYSTEM  WILL  BE  OBSERVED 

SMODE  =  NUMBER  OF  STARTING  MODE  (INT) 

MODE  =  NUMBER  OF  MODES  (INT) 

EMODE  =  NUMBER  OF  THE  LAST  MODE  (INT) 

NODE  =  NUMBER  OF  THE  NOISE  INPUT  MODE  (INT) 

***  NOISE  SLOPE  LOCATIONS  IN  DATA  MATRIX  *** 

ROWN1  =  X- SLOPE  LOCATION 

ROWN2  =  Y-SLOPE  LOCATION 

ROWN3  =  Z-SLOPE  LOCATION 

C  =  OUTPUT  MATRIX  FOR  Y 

IDENT  =  IDENTITY  MATRIX 

RMN  =  MEASUREMENT  NOISE  COVARIANCE  MATRIX 

QPN  =  PLANT  NOISE  COVARIANCE  MATRIX 

PNVARX  =  PLANT  NOISE  X-SLOPE  VARIANCE 

PNVARY  =  PLANT  NOISE  Y-SLOPE  VARIANCE 

PNVARZ  =  PLANT  NOISE  Z-SLOPE  VARIANCE 

MNVARX  =  MEASUREMENT  NOISE  X-SLOPE  VARIANCE 

MNVARY  =  MEASUREMENT  NOISE  Y-SLOPE  VARIANCE 

MNVARZ  =  MEASUREMENT  NOISE  Z-SLOPE  VARIANCE 

I SEED  =  INITIALIZATION  FOR  RANDOM  NUMBER  GENERATOR 

XKAL  =  X  MATRIX 

Y  =  OUTPUT  MATRIX 

RNDM  «  RANDOM  NUMBERS  USED  FOR  WHITE  NOISE  IN  MEASUREMENTS  AND 
IN  PLANT  FORCES 

BN  =  B  MATRIX  TO  MULTIPLY  NOISE  DISTURBANCES 
TNX,TNY,TNZ=  NOISE  TORQUES  X,Y,Z  SLOPES 
M2=2*MODE 


**********  SAMPLE  OF  SPACE  EXEC  FILE  *************** 

THIS  FILE  MUST  BEGIN  IN  COLUMN  1  AND  RUN  WITH  THE  FOLLOWING 
SEQUENCE  FOR  THE  INITIAL  RUN  OF  THE  PROGRAM: 

FORTVS  SPACE  (COMPILES  PROGRAM) 

SPACE  (EXECUTES  EXEC  FILE) 

LOAD  SPACE  (START  (LOADS  AND  EXECUTES  PROGRAM) 

SUBSEQUENT  PROGRAM  RUNS  CAN  ELIMINATE  "FORTVS  SPACE"  IF  NO 
CHANGES  HAVE  BEEN  MADE  TO  THE  PROGRAM,  AND  CAN  ELIMINATE 
RUNNING  THE  EXEC  FILE. 

FI  4  DISK  THESIS  INPUT  B  (PERM 

FI  8  DISK  UTILITY  DATA  (RECFM  VS  BLOCK  133  PERM 

FI  11  DISK  CNTRL  OUTPUT  (RECFM  F  BLOCK  80  LRECL  80  PERM 

FI  13  DISK  GAMMA  OUTPUT  (RECFM  VS  BLOCK  133  PERM 

FI  14  DISK  MODE  OUTPUT  (RECFM  F  BLOCK  80  LRECL  80  PERM 


SIM00520 

SIM00530 

SIM00540 

SIM00550 

SIM00560 

SIM00570 

SIM00580 

SIM00590 

SIM00600 

SIM00610 

SIM00620 

SIM00630 

SIM00640 

SIM00650 

SIM00660 

SIM00670 

SIM00680 

SIM00690 

SIM00700 

SIM00710 

SIM00720 

SIM00730 

SIM00740 

SIM00750 

SIM00760 

SIM00770 

SIM00780 

SIM00790 

SIM00800 

SIM00810 

SIM00820 

SIM00830 

SIM00840 

SIM00850 

SIM00860 

SIM00870 

SIM00880 

SIM00890 

SIM00900 

SIM00910 

SIM00920 

SIM00930 

SIM00940 

SIM00950 

SIM00960 

SIM00970 

SIM00980 

SIM00990 

SIM01000 

SIM01010 

SIM01020 

SIM01030 

SIM01040 

SIM01050 

SIM01060 

SIM01070 


50 


« 


c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


c 

c 

c 

c 

c 


c 

c 

c 

c 


5 

c 

1001 

1002 

1008 

C 

500 

C 

C 

C 

C 

700 

C 

C 

C 

C 

C 

C 


FI  16  DISK  COST  OUTPUT  (RECFM  F  BLOCK  80  LRECL  80  PERM 

FI  17  DISK  PRT  OUTPUT  (RECFM  F  BLOCK  80  LRECL  80  PERM 

FI  18  DISK  ERROR  DATA  (RECFM  F  BLOCK  80  LRECL  80  PERM 

FI  19  DISK  END  FILE  (RECFM  F  BLOCK  80  LRECL  80  PERM 

FI  20  DISK  GMAT  FILE  (RECFM  F  BLOCK  80  LRECL  80  PERM 


PARAMETER  (JR=5243,  KR=5397,  MR=262139) 


MIN  =1.  00 
WT=1.  ODOO 

PI  =  4. ODO  *  ATAN( 1. ODO) 


*****  READ  LAMA  AND  UGVEX  MATRICIES 

************************* 


***** 


CALL  EXCMS  ( ' CLRSCRN' ) 

WRITE(6,1008) 

WRITE( 6 ,*)  '  READING  LAMA  AND  UGVEX  MATRICIES' 

WRITE(6,*)  '  ' 

THIS  SECTION  READS  THE  LAMA  VECTOR  AND  THE  UGVEX 
MATRIX  AND  STORES  THEM  IN  MEMORY  FOR  FURTHER  RECALL  OF 
DESIRED  LOCATION  DATA. 

READ(4, 1001)  NAM 

READ( 4 , 1002 ) ( LAMA( I ) , 1=1 , 100) 

READ(4, 1001)  NAM 
DO  5  J  =  1,100 

READ(4, 1002)(UGVEX( I , J) , 1=1,684) 

CONTINUE 

FORMAT( 1X,A6) 

FORMAT( 1X.8E15.  8) 

FORMAT  (IX,////) 

CALL  EXCMS  ('CLRSCRN') 

************  STARTING  MODE  NUMBER  ************** 

**  SMODE  7  TO  100  (INTEGER)  **** 

SMODE=  7 

WRITE  (16,700)  SMODE 

FORMAT  ('  '.'STARTING  MODE  NUMBER:  ’,12) 

***************  NUMBER  OF  MODES  TO  SCAN  ************* 

**  MODE  1  TO  93  (INTEGER) 

M0DE=20 


EMODE  =  SMODE  +  MODE  *  1 


SIM01080 

SIM01090 

SIM01100 

SIM01110 

SIM01120 

SIMC1130 

SIM01140 

SIM01150 

SIM01160 

SIM01170 

SIM01180 

SIM01190 

SIM01200 

SIM01210 

SIM01220 

SIM01230 

SIM01240 

SIM01250 

SIM01260 

SIM01270 

SIM01280 

SIM01290 

SIM01300 

SIM01310 

SIM01320 

SIM01330 

SIM01340 

SIM01350 

SIM01360 

SIM01370 

SIM01380 

SIM01390 

SIM01400 

SIM01410 

SIM01420 

SIM01430 

SIM01440 

SIM01450 

SIM01460 

SIM01470 

SIM01480 

SIM01490 

SIM01500 

SIM01510 

SIM01520 

SIM01530 

SIM01540 

SIM01550 

SIM01560 

SIM01570 

SIM01580 

SIM01590 

SIM01600 

SIM01610 

SIM01620 
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WHITE  (16,701)  MODE 

701  FORMAT  ('  NUMBER  OF  MODES  SCANNED:  ',12) 

C 

C  ************  NOISE  INPUT  POSITION  ***************** 
C  **  NODE  1  TO  114  (INTEGER)  (IF  0  THEN  NO  NOISE  INPUT) 

NODE=  8 
C 

WRITE  (16,702)  NODE 

702  FORMAT  ('  NOISE  NODE  LOCATION:  *,I5) 

C 

C  ***********  START  AND  STOP  FOR  PLANT 
SN=7 
FN=20 
NS=SN*2 - 1 
NF=SN*2+2*FN - 2 
WRITE  (16,899)  SN,FN 

899  FORMAT  ('  PLANT  --  SN=  ',15,'  FN=  ’,15) 

C  *************  SAMPLING  TIME  ************** 

C  **  SAMPT  MUST  BE  LESS  THAN  OR  EQUAL  TO  SAMPTM  ** 

SAMPT  =  0.  05 

SAMPTM  =  ((2.  ODO*PI)/SQRT(LAMA(EMODE) ))/l. 0D01 
IF  (SAMPT. GE. SAMPTM)  THEN 
SAMPT=SAMPTM 

ENDIF 

C 

WRITE  (16,900)  MIN 

900  FORMAT  ('  ’,2X,'MIN:  ' ,F8. 3) 

C 

WRITE  (16,703)  SAMPT,  SAMPTM 

703  FORMAT  ('  SAMPLING  TIME:  ’ ,D12.  4,2X, ’SAMPTM=  ’ ,D15. 8) 

C 

C  ***************  DAMPING  FACTOR  ************** 

C  **  DAMP  0.0  TO  1.0  (REAL*8) 

DAMP=. 01 
C 

WRITE  (16,704)  DAMP 

704  FORMAT  (’  DAMPING  FACTOR:  ’,D12.4) 

C 

C 

C  ***  PLANT  NOISE  VARIANCE  *** 

C  **  PNVARX,  PNVARY ,  PNVARZ  GT  0.  0 
SF1=2. 5D06 
SF=1. ODOO 
C 

PNVARX=1. ODOO-SFi 
PNVARY=1.  0D00*SF1 
PNVARZ=1.  0D00*SF1 
C 
C 
C 

C  ***  MEASUREMENT  NOISE  VARIANCE  *** 

C  **  MNVARX,  MNVARY,  MNVARZ  GT  0.0 
MNVARX=1. 0D-03  *SF 
MNVARY=1.  0D-03  *SF 


SIM01630 

SIM01640 

SIM01650 

SIM01660 

SIM01670 

SIM01680 

SIM01690 

SIM01700 

SIM01710 

SIM01720 

SIM01730 
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SIM01770 

SIM01780 

SIM01790 

SIM01800 

SIM01810 

SIM01820 

SIM01830 

SIM01840 

SIM01850 

SIM01860 

SIM01870 

SIM01880 

SIM01890 

SIM01900 

SIM01910 

SIM01920 

SIM01930 

SIM01940 
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MNVARZ=1.  0D-03  *SF 
C 
C 

WRITE  (16.711) 

711  FORHATC  V  PLANT  NOISE  VARIANCE:  ’  ) 

WRITE  (16,712) 

712  FORMATC  ' ,6X, 'PNVARX1 ,13X, 'PNVARY' ,13X, 'PNVARZ' ) 

WRITE  (16,713)  PNVARX,  PNVARY,  PNVARZ 

713  FORMATC  ' .2X.E15.  8,2X,E15.  8,2X,E15.  8) 

WRITE( 16,714) 

7 14  FORMAT( '  ' , ' MEASUREMENT  NOISE: ' ) 

WRITE( 16,715) 

715  FORMATC  ' ,6X, 'MNVARX' , 13X, 'MNVARY' , 13X, 'MNVARZ' ) 

WRITE( 16,713)  MNVARX, MNVARY, MNVARZ 

C 

510  CALL  EXCMS  ('CLRSCRN') 

WRITE  (6,1008) 

WRITE  (6,*)  '  PROGRAM  RUNNING' 

C 

C  ***********  NOISE  INPUT  LOCATION  ************ 

C 

R0WN3  =  N0DE*6 
R0WN2  =  (NODE* 6)  -  1 
ROWN1  =  (NODE*6)  -  2 
COUNT  =  0 
C 
C 

Q  ***********  INITIALIZE  MATRICIES  ****** ********** 

C 

DO  48  K=1 ,50 

IDENT(K,K)=1.  0 
48  CONTINUE 

C 

DO  54  K  =  1,  200 
X(K)  -  0.  0 
54  CONTINUE 

C 
C 

WRITE( 6 , 1008) 

WRITE  (6,*)  '  INITIALIZE  RMN  AND  QPN  MATRICES 
C  ***  INITIALIZE  RMN  AND  QPN  MATRICES  *** 

C 

if  DO  60  1=1,3 

DO  58  J=1 , 3 
RMN( I , J)=0. 0 
QPN( I , J)=0. 0 
58  CONTINUE 

60  CONTINUE 

C 

RMN(1,1)=MNVARX**2 
RMN( 2,2)=MNVARY**2 
RMN( 3 , 3)=MNVARZ**2 
QPN( 1 , 1)=PNVARX**2 
QPN( 2 , 2)=PNVARY**2 
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c 

c 


c 

c 

c 

c 

c 


c 

c 


QPN(3,3)=PNVARZ**2.  0 


WRITE(6, 1008) 

WRITE(6,*)  '  ENTER  STMTRX 


***** 


BEGIN  MAIN  PROGRAM 


***** 


CALL  STMTRX( EMODE , SMODE , S AMPT , DAMP , PH I , GAMMA , A , B , LAMA , UGVEX , C , 
•f  R0WN1,R0WN2,R0WN3,BN) 


WRITE  (16,1008) 

DO  11000  1=1,200 
DO  10900  J=1 , 3 
C(J,I)=  C(  J,  I)*SF 
10900  CONTINUE 
11000  CONTINUE 
C 

C  ***  PRE-LOOP  PORTION  OF  KALMAN  FILTER 

C 

M2=2*M0DE 
JP=2*SM0DE - 1 
JQ=2*EMODE 
DO  90  1=1,50 
XH( I)=0.  0 
90  CONTINUE 

C 

DO  9971  1=1, M2 

READ  (20,*)  G( I , 1) ,  G( I ,2) ,  G(I,3) 
9971  CONTINUE 
C 
C 

WRITE  (14,1008) 

DO  384  1=1, M2 

WRITE  (14,5350)  G( I , 1) ,G( I ,2) ,G( I , 3) 

384  CONTINUE 

5350  FORMAT  ('  ' ,2X,D15. 8.2X.D15.  8,2X,D15. 8) 
C 
C 
C 

C  ***** 

C 


THIS  SECTION  COMPUTES  THE  STATE  UPDATE 


***** 


DO  9771  1=1,100 
E2( I)=0.  0 
E3(I)=0.  0 
XS(I)=0.0 
9771  CONTINUE 
ESS  =0.0 
COUNT  =  0 
ENERGY  =  0.0D0 
TIME  =  0.0 
CGN=0. 0 
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CTG=0.  0 

SIM03260 

c 

*****  SETS  LOOP  FOR  THE  ITERATIONS  NECESSARY  TO  OBSERVE  ***** 

SIM03270 

c 

*****  the  SYSTEM  FOR  THE  NUMBER  OF  MINUTES  SPECIFIED  ***** 

SIM03280 

WRITE  (6,1008) 

SIM03290 

WRITE  (6,*)  '  START  STATE  UPDATE  ' 

SIM03300 

LOOP  =  INT((MIN*60.  0)/SAMPT) 

SIM03310 

PRT=  (DBLE(LOOP) )/30.  0 

SIM03320 

CTT=0.  0 

SIM03330 

c 

SIM03340 

DO  400  L  =  0,  LOOP 

SIM03350 

TIME  =  DBLE(L)*SAMPT 

SIM03360 

c 

SIM03370 

IF(L.  EQ.  0)THEN 

SIM03380 

IMPLSE  =( 1.  0D06*SF1)/(DSQRT(SAMPT)) 

SIM03390 

ELSE 

SIM03400 

IMPLSE  =  0.  0D0 

SIM03410 

ENDIF 

SIM03420 

c 

SIM03430 

T0=0. 0 

SIM03440 

c 

*****  RANDOM  NUMBER  GENERATOR  ***** 

SIM03450 

c 

SIM03460 

DO  101  1=1,6 

SIM03470 

ISEED=MOD( I SEED* JR+KR , MR ) 

SIM03480 

RND1=(DBLE( ISEED)+0.  5D00)/DBLE(MR) 

SIM03490 

I SEED=MOD( I SEED* JR+KR , MR ) 

SIM03500 

RND2=( DBLE ( I SEED ) +0 .  5D00)/DBLE(MR) 

SIM03510 

RNDM(I)=DSQRT(~2. 0*DL0G ( RND 1 ) ) *DCOS ( 6 . 2831853D00*RND2) 

SIM03520 

101 

CONTINUE 

SIM03530 

c 

******************* 

SIM03540 

CTT=CTT+1.  0 

SIM03550 

c 

****  START  OF  STATE  UPDATE  *** 

SIM03560 

c 

SIM03570 

c 

***  COMPUTE  AX°200  =  A0 200  X  200  *  X#200 

SIM03580 

c 

SIM03590 

c 

SIM03600 

c 

***  COMPUTE  AX  =  A*X 

SIM03610 

c 

SIM03620 

JK=SMODE*2-2 

SIM03630 

JP=JK+1 

SIM03640 

JQ=2*EMODE 

SIM03650 

c 

SIM03660 

c 

SIM03670 

DO  5015  I=NS,NF 

SIM03680 

SUM=0. 0 

SIM03690 

DO  5010  K=NS,NF 

SIM03700 

SUM=SUM+A(I,K)*X(K) 

SIM03710 

5010 

CONTINUE 

SIM03720 

AX(I)=SUM 

SIM03730 

5015 

CONTINUE 

SIM03740 

C 

SIM03750 

C 

***  COMPUTE  WD°3 

SIM03760 

C 

SIM03770 

WD( 1)=PNVARX*RNDM( l)*TO+IMPLSE 

SIM03780 

WD( 2 ) =PNVARY*RNDM( 2 ) *TO 

SIM03790 

WD( 3)=PNVARZ*RNDM( 3 )*TO 

SIM03800 

c 

SIM03810 
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c 

SIM03820 

c 

***  COMPUTE  BNWD°200  =BN°200  X  3  *  WD#3 

SIM03830 

c 

SIM03840 

DO  5035  I=NS,NF 

SIM03850 

SUM=0.  0 

SIM03860 

DO  5030  K=l,3 

SIM03870 

SUM=SUM+BN( I ,K)*WD(K) 

SIM03880 

5030 

CONTINUE 

SIM03890 

BNWD(I)=SUM 

SIM03900 

5035 

CONTINUE 

SIM03910 

C 

SIM03920 

C 

***  COMPUTE  X#200  =AX°200  +  BNVD8200 

SIM03930 

C 

SIM03940 

DO  5040  I=NS,NF 

SIM03950 

X(I)=  AX( I)  +  BNWD(I) 

SIM03960 

IF  (DABS(X(I)).LT.  1. 0D-60)  THEN 

SIM03970 

X(I)=1. 0D-60 

SIM03980 

END  IF 

SIM03990 

C 

SIM04000 

C 

SIM04010 

C 

SIM04020 

5040 

CONTINUE 

SIM04030 

C 

SIM04040 

C 

***  COMPUTE  V°3 

SIM04050 

C 

SIM04060 

V( 1 ) =MNVARX*RNDM( 4 ) 

SIM04070 

V( 2 ) =MNVARY*RNDM( 5 ) 

SIM04080 

V( 3 ) =MNVARZ*RNDM( 6 ) 

SIM04090 

C 

SIM04100 

C 

***  COMPUTE  Y° 3  =  C°3  X  200  *  X°200  +  V°3 

SIM04110 

c 

SIM04120 

DO  5050  1=1,3 

SIM04130 

SUM=0. 0 

SIM04140 

DO  5045  K=NS,NF 

SIM04150 

SUM=SUM+C( I ,K)*X(K) 

SIM04160 

5045 

CONTINUE 

SIM04170 

Y( I)=SUM+V( I) 

SIM04180 

5050 

CONTINUE 

SIM04190 

C 

SIM04200 

C 

******************** 

SIM04210 

C 

SIM04220 

C 

***  START  OF  KALMAN  FILTER  *** 

SIM04230 

c 

SIM04240 

M2=2*M0DE 

SIM04250 

c 

SIM04260 

c 

SIM04270 

c 

***  COMPUTE  XH1  =  A*XH 

SIM04280 

c 

SIM04290 

DO  300  I=JP, JQ 

SIM04300 

SUM=0. 0 

SIM04310 

DO  295  K=JP, JQ 

SIM04320 

SUM=SUM+A( I ,K)  *  XH(K) 

SIM04330 

295 

CONTINUE 

SIM04340 

XH1( I)=SUM 

SIM04350 

300 

CONTINUE 

SIM04360 

C 

SIM04370 
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c 

SIM04380 

c 

***  COMPUTE  DY  =  Y  -  C*XH1 

SIM04390 

c 

SIM04400 

DO  315  1=1,3 

SIM04410 

SUM=0. 0 

SIM04420 

DO  310  K=JP,JQ 

SIM04430 

SUM=SUM+C ( I , K ) *XH 1 ( K ) 

SIM04440 

310 

CONTINUE 

SIM04450 

DY(I)=Y(I)-SUM 

SIM04460 

315 

CONTINUE 

SIM04470 

C 

SIM04480 

C 

***************** 

SIM04490 

C 

SIM04500 

C 

***  COMPUTE  XH  =  XH1  +  G*DY 

SIM04510 

C 

SIM04520 

DO  325  1=1, M2 

SIM04530 

J1=JP-1+I 

SIM04540 

SUM=0. 0 

SIM04550 

DO  320  K=l,3 

SIM04560 

SUM=SUM+G( I ,K)*DY(K) 

SIM04570 

320 

CONTINUE 

SIM04580 

XH(J1)=XH1(J1)+SUM 

SIM04590 

IF  (DABS(XH( Jl) ).  LT.  1.  0D-60)  THEN 

SIM04600 

XH( Jl)=l. 0*D-60 

SIM04610 

END  IF 

SIM04620 

C 

SIM04630 

325 

CONTINUE 

SIM04640 

C 

SIM04650 

C 

SIM04660 

C 

SIM04670 

C 

*****  END  OF  KALMAN  ROUTINES  ****** 

SIM04680 

C 

SIM04690 

C 

***  COMPUTATION  OF  ESUM  *** 

SIM04700 

DO  340  I=JP,JQ 

SIM04710 

XDEL=  X(I)-XH(I) 

SIM04720 

XDEL1=XDEL*XDEL*SAMPT 

SIM04730 

E2(I)=E2(I)+XDEL1 

SIM04740 

XS( I )=XS( I )+X( I )*X( I )*SAMPT 

SIM04750 

E3( I)=E2( I)/XS(I) 

SIM04760 

340 

CONTINUE 

,  SIM04770 

C 

SIM04780 

CGN=CGN+1.  0 

SIM04790 

IF  (CTT.EQ.  1.  0.  OR.CGN.  GT.  PRT)  THEN 

SIM04800 

C 

SIM04810 

WRITE  (6,*)  ' TIME=  ',  TIME,  '  SEC.’ 

SIM04820 

C 

SIM04830 

WRITE  (17,1008) 

SIM04840 

WRITE  (16,1008) 

SIM04850 

WRITE  (16,2100)  TIME 

SIM04860 

C 

SIM04870 

WRITE  (17,2100)  TIME 

SIM04880 

2100 

FORMATC  '  ,  ’TIME=  ' ,F9.3) 

SIM04890 

DO  380  I=JP  ,  JQ 

SIM04900 

WRITE  (16,4500)  I,X(I)  ,1  ,XH(I) 

SIM04910 

C 

SIM04920 
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WRITE  (17,4530)  I,E2(I)  ,E3(I)  ,  XS(I) 

SIM04930 

380 

CONTINUE 

SIM04940 

C 

SIM04950 

C 

SIM04960 

C 

SIM04970 

C 

SIM04980 

CGN=0.  0 

SIM04990 

END  IF 

SIM05000 

4500 

FORMAT  ('  X( ' , 13, ' )=  ' ,D15. 8,2X, 'XH( ’ ,13, ' )=  ' ,D15. 8) 

SIM05010 

4530 

FORMAT  ('  ' ,5X,I5,5X,3  D15. 8) 

SIM05020 

C 

SIM05030 

400 

CONTINUE 

SIM05040 

C 

SIM05050 

C 

SIM05060 

DO  401  I=JP, JQ 

SIM05070 

WRITE  (19,4530)  I,  E2(I)  ,E3(I),  XS(I) 

SIM05080 

401 

CONTINUE 

SIM05090 

C 

SIM05100 

C 

SIM05110 

C 

SIM05120 

C 

SIM05130 

599 

STOP 

SIM05140 

END 

SIM05150 

C 

SIM05160 

C 

SIM05170 

C 

*********************************************■*-***************** 

SIM05180 

C 

THIS  SUBROUTINE  COMPUTES  THE  STATE  TRANSITION  MATRIX  FOR  EACH 

SIM05190 

C 

OF  THE  100  MODES 

SIM05200 

C 

■)lriV*************************->V****iV**ilr***iV**>ViV****->V********5V**5V** 

SIM05210 

C 

SIM05220 

SUBROUTINE  STMTRX( EMODE , SMODE ,T , D , PHI , GAMMA , A , B , LAMA , UGVEX , C , 

SIM05230 

+  ROWN 1 , ROWN  2 , ROWN  3 , BN ) 

SIM05240 

C 

SIM05250 

REAL*8  WN,GMA,PHI( 2 ,2,100) ,GAMMA( 2, 100) ,EGT,T,C0SW1T,SINW1T 

SIM05260 

REAL*8  W1,D,A(200,200),B(200,3),C(3,200),BN(200,3) 

SIM05270 

REAL  LAMA( 100) ,UGVEX(684, 100) 

SIM05280 

INTEGER  SMODE , R , EMODE , JJ , KK , ROWN 1 , ROWN 2 , ROWN 3 

SIM05290 

C 

SIM05300 

c 

SIM05310 

DO  600  I  =  1  ,100 

SIM05320 

WN  =  DBLE(SQRT(LAMA(I))) 

SIM05330 

GMA  =  D*WN/2. 0 

SIM05340 

EGT  =  DEXP( -GMA*T) 

SIM05350 

W1  =  DSQRT((WN**2)-(GMA**2)) 

SIM05360 

COSW1T  =  DC0S(W1*T) 

SIM05370 

SINW1T  =  DSIN(W1*T) 

SIM05380 

c 

SIM05390 

c 

SIM05400 

c 

SIM05410 

c 

SIM05420 

IF(WN. EQ.  0)THEN 

SIM05430 

PHI( 1,1,1)  =  EGT*COSW IT 

SIM05440 

PHI( 1,2,1)  =  T 

SIM05450 

PHI (2,1,1)  =  0 

SIM05460 

PHI( 2,2,1)  =  EGT*C0SW1T 

SIM05470 

c 

SIM05480 
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ooooooo  ooooo  n  ooo'ooo  oooo  oooo  oooo  o  o  o  o  o 


GAMMA( 1,1)  =  0 
GAMMA( 2,1)  =  0 

ELSE 


PHI( 1,1,1)  =  EGT*(C0SW1T  +  (GMA*(W1**( -1) ) )*SINW1T) 

PHI(  1,2,1)  =  (Wl**( -1) )*EGT*SINW1T 

PHI( 2, 1,1)  =  -(WN**2)*(W1**(-1))*EGT*SINW1T 

PHI (2, 2, I)  =  EGT*(C0SW1T  -  (GMA*(W1**( -1) ) )*SINW1T) 


GAMMA(1,I)=(WN*‘H-2))*(1-ODO-EGT*COSW1T  -EGT*(GMA/W1)*S INWIT) 
GAMMA(  2,1)  =  (W1**C-1))*EGT*SINW1T 


END  IF 


00  CONTINUE 


R  =  1 

DO  610  K  =  1  ,100 


A(R,R)  =  PHI( 1 , 1 ,K) 
A(R,R+1)  =  PHI( 1 , 2 ,K) 
A(R+1,R)  =  PHI( 2 , 1 ,K) 
A(R+1,R+1)  =  PHI( 2 , 2 ,K) 


***  B  MATRIX  FOR  MULTIPLYING  U0NTR0L  TORQUES 

B(R, 1)  =  GAMMA( 1 ,K)*DBLE(UGVEX(412 ,K) ) 
B(R,2)  =  GAMMA(1,K)*DBLE(UGVEX(413,K)) 
B(R,3)  =  GAMMA(1,K)*DBLE(UGVEX(414,K)) 
B(R+1 , 1)  =  GAMMA( 2 ,K)*DBLE(UGVEX(412 ,K) ) 
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SIM05840 

SIM05850 

SIM05860 

SIM05870 

SIM0588C 

SIM05890 

SIM05900 
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noa<n  n  ono  non  n  o  n  o  n  n  o\  nonono  ooooooooooon 


B(R+1 , 2)  =  GAMMA( 2 , K) *DBLE ( UGVEX( 4 1 3 , K ) ) 
B(R+1 ,3)  =  GAMMA(2,K)*DBLE(UGVEX(414,K)) 


***  BN  MATRIX  FOR  MULTIPLYING  THE  NOISE  DISTURBANCES 


BN(R,1)=GAMMA(1,K)*DBLE(UGVEX(R0WN1,K)) 
BN(R,2)=GAMMA( 1 ,K)*DBLE(UGVEX(R0WN2 ,K) ) 
BN(R,3)=GAMMA( 1 ,K)*DBLE(UGVEX(R0WN3 ,K) ) 
BN( R+ 1 , 1 ) =GAMMA( 2 ,K)*DBLE(UGVEX( ROWN 1 , K ) ) 
BN( R+l , 2 ) =GAMMA( 2 , K ) *DB LE ( UG VEX( R0WN2 , K ) ) 
BN( R+ 1 , 3 ) =GAMMA( 2 , K ) *DB  L£ ( UG VEX( R0WN3 , K ) ) 


R  =  R+2 
10  CONTINUE 

***  C  MATRIX  PRODUCTION  *** 


JJ=-1 

DO  640  1=1,100 
JJ=JJ+1 

KK=I  +JJ 


C(1,KK)  =  DBLE(UGVEX(418 , I ) ) 
C( 2 ,KK)  =  DBLE(UGVEX(419 , I) ) 
C( 3 ,KK)  =  DBLE(UGVEX(420, I) ) 


KK=KK+1 

C( 1 ,KK)=0.  0 
C(2,KK)=0.  0 
C( 3 ,KK)=0. 0 

40  CONTINUE 


SIM06050 

SIM06060 

SIM06070 

SIM06080 

SIM06090 

SIM06100 

SIM06110 

SIM06120 

SIM06130 

SIM06140 

SIM06150 

SIM06160 

SIM06170 

SIM06180 

SIM06190 

SIM06200 

SIM06210 

SIM06220 

SIM06230 

SIM06240 

SIM06250 

SIM06260 

SIM06270 

SIM06280 

SIM06290 

SIM06300 

SIM06310 

SIM06320 

SIM06330 

SIM06340 

SIM06350 

SIM06360 

SIM06370 

SIM06380 

SIM06390 

SIM06400 

SIM06410 

SIM06420 

SIM06430 

SIM06440 

SIM06450 

SIM06460 

SIM06470 

SIM06480 

SIM06490 

SIM06500 

SIM06510 

SIM06520 

SIM06530 

SIM06540 

SIM06550 

SIM06560 

SIM06570 

SIM06580 

SIM06590 

SIM06600 
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u  o 


RETURN 

END 


SIM06610 

SIM06620 

SIM06630 

SIM06640 
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APPENDIX  C.  PROGRAM  TO  ESTIMATE  NOISE  IN  KALMAN  FILTER 

FROM  UNOBSERVED  MODES 


*****  SPAC  24  ***** 
*****  ADAPTED  TO  RUN  N  MODES  OF  THE  PLANT  AND  ***** 
*****  COMPUTE  THE  NOISE  IN  THE  KALMAN  FILTER  ***** 
*****  FROM  THE  UNOBSERVED  MODES  ***** 


***** 


VARIABLE  DECLARATIONS 


***** 


EXTERNAL  STMTRX.EXCMS 
CHARACTER*6  NAM 

CHARACTER* 1  AGAIN, CORECT , RAGAIN 

INTEGER  R0WN1 , ROWN2 , R0WN3 , COUNT , NODE , MODE , KQ , EMODE , SMODE , R2M , C2M 
INTEGER  CT , CF , KAD J , CFAD J , LOOP , PRNT , J J , JK , N1 , JR , KR , MR , I SEED , M2 
INTEGER  NO,  NS,  NF,  SN,  FN 

INTEGER  JL,J1,JM  ,  JP,  JQ,  KA,  KB,  KC,  KD,  KE,  KF,  KG 


REAL  LAMA(IOO),  UGVEX(684,10O),RNODE,RMODE,MIN 
REAL*8  PHI(2 ,2 , 100) ,GAMMA( 2 , 100) ,EGT,GMA,WN,W1 ,X1T,X2T,TIME 
REAL*8  A( 200,200), B(200, 3), F(3,  50) , IMPLSE .ENERGY 
REAL*8  COSW1T,SINW1T,X(200) 

REAL*8  DAMP , SAMPT , PI , SAMPTM , SUM1 , SUM2 , SUM3 , SUMC 
REAL*8  C(9 ,200) ,  RMN(3,3),  QPN(3,3) 

RF.AL*8  BN( 200 ,3) 

REAL*8  PNVARX,  PNVARY ,  PNVARZ 
REAL*8  MNVARX,  MNVARY,  MNVARZ 
REAL*8  SUM,  RNDM(6) ,  RND1 ,  RND2 
REAL*8  ES , ED , ESUM , CGN , PRT 
REAL*8  WT  ,  WD(3) ,  BNWD(200),  EX1(9) 

REAL*8  EX(9) ,AX(200)  ,  SF  ,  TO,  CTT,  ESS 
REAL*8  CTG,  XDEL,  XDEL1,  ERS,  PRT1 
REAL*8  SF1 


STMTRX  =  SUBROUTINE  EXTABLISHES  STATE  TRANSITION  MATRICIES 
LAMA  =  VECTOR  OF  THE  SQUARE  OF  THE  NATURAL  FREQUENCIES 
UGVEX  =  MODE  POSITONS  AND  SLOPES  OF  THE  NODAL  POINTS 
PHI  =  STATE  TRANSITION  MATRICIES  FOR  EACH  MODE 
GAMMA  =  INPUT  TRANSITION  MATRIX 
A  =  DIAGONAL  MATRIX  CONSISTING  OF  PHI 


SPA00010 

SPA00020 

SPA00030 

SPA00040 

SPA00050 

SPA00060 

SPA00070 

SPA00080 

SPA00090 

SPA00100 

SPA00110 

SPA00120 

SPA00130 

SPA00140 

SPA00150 

SPA00160 

SPA00170 

SPA00180 

SPA00190 

SPA00200 

SPA00210 

SPA00220 

SPA00230 

SPA00240 

SPA00250 

SPA00260 

SPA00270 

SPA00280 

SPA00290 

SPA00300 

SPA00310 

SPA00320 

SPA00330 

SPA00340 

SPA00350 

SPA00360 

SPA00370 

SPA00380 

SPA00390 

SPA00400 

SPA00410 

SPA00420 

SPA00430 

SPA00440 

SPA00450 

SPA00460 

SPA00470 

SPA00480 

SPA00490 
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B  =  INPUT  MATRIX  OF  GAMMA  AND  CONTROL  SLOPES 

DAMP  =  DAMPING  FACTOR 

SAMPT  =  SAMPLING  TIME 

IMPLSE  =  IMPULSE  INPUT  FUNCTION 

MIN  =  NUMBER  OF  MINUTES  SYSTEM  WILL  BE  OBSERVED 

SMODE  =  NUMBER  OF  STARTING  MODE  (INT) 

MODE  =  NUMBER  OF  MODES  (INT) 

EMODE  =  NUMBER  OF  THE  LAST  MODE  (INT) 

NODE  =  NUMBER  OF  THE  NOISE  INPUT  MODE  (INT) 

***  NOISE  SLOPE  LOCATIONS  IN  DATA  MATRIX  *** 

ROWN1  =  X-SLOPE  LOCATION 
ROWN2  =  Y-SLOPE  LOCATION 
ROWN3  =  Z- SLOPE  LOCATION 
C  =  OUTPUT  MATRIX  FOR  Y 
IDENT  =  IDENTITY  MATRIX 
RMN  =  MEASUREMENT  NOISE  COVARIANCE  MATRIX 
QPN  =  PLANT  NOISE  COVARIANCE  MATRIX 
PNVARX  =  PLANT  NOISE  X-SLOPE  VARIANCE 
PNVARY  =  PLANT  NOISE  Y-SLOPE  VARIANCE 
PNVARZ  =  PLANT  NOISE  Z- SLOPE  VARIANCE 
MNVARX  =  MEASUREMENT  NOISE  X-SLOPE  VARIANCE 
MNVARY  =  MEASUREMENT  NOISE  Y-SLOPE  VARIANCE 
MNVARZ  =  MEASUREMENT  NOISE  Z-SLOPE  VARIANCE 
I SEED  =  INITIALIZATION  FOR  RANDOM  NUMBER  GENERATOR 
RNDM  =  RANDOM  NUMBERS  USED  FOR  WHITE  NOISE  IN  MEASUREMENTS  AND 
IN  PLANT  FORCES 

BN  =  B  MATRIX  TO  MULTIPLY  NOISE  DISTURBANCES 


**********  SAMPLE  OF  SPACE  EXEC  FILE  ****************** 

THIS  FILE  MUST  BEGIN  IN  COLUMN  1  AND  RUN  WITH  THE  FOLLOWING 
SEQUENCE  FOR  THE  INITIAL  RUN  OF  THE  PROGRAM: 

FORTVS  SPACE  (COMPILES  PROGRAM) 

SPACE  (EXECUTES  EXEC  FILE) 

LOAD  SPACE  (START  (LOADS  AND  EXECUTES  PROGRAM) 

SUBSEQUENT  PROGRAM  RUNS  CAN  ELIMINATE  "FORTVS  SPACE"  IF  NO 
CHANGES  HAVE  BEEN  MADE  TO  THE  PROGRAM,  AND  CAN  ELIMINATE 
RUNNING  THE  EXEC  FILE. 

FI  4  DISK  THESIS  INPUT  (PERM 

FI  8  DISK  UTILITY  DATA  (RECFM  VS  BLOCK  133  PERM 
FI  11  DISK  CNTRL  OUTPUT  (RECFM  F  BLOCK  80  LRECL  80  PERM 
FI  13  DIlT  GAMMA  OUTPUT  (RECFM  VS  BLOCK  133  PERM 
FI  14  DISK  MODE  OUTPUT  (RECFM  F  BLOCK  80  LRECL  80  PERM 

FI  16  DISK  COST  OUTPUT  (RECFM  F  BLOCK  80  LRECL  80  PERM 

FI  17  DISK  PRT  OUTPUT  (RECFM  F  BLOCK  80  LRECL  80  PERM 

FI  18  DISK  ERROR  DATA  (RECFM  F  BLOCK  80  LRECL  80  PERM 

FI  19  DISK  END  FILE  (RECFM  F  BLOCK  80  LRECL  80  PERM 
FI  20  DISK  GMAT  FILE  (RECFM  F  BLOCK  80  LRECL  80  PERM 
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SPA00690 

SPA00700 
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SPA00730 

SPA00740 

SPA00750 

SPA00760 

SPA00770 

SPA00780 

SPA00790 

SPA00800 

SPA00810 

SPA00820 

SPA00830 

SPA00840 

SPA00850 

SPA00860 

SPA00870 

SPA00880 

SPA00890 

SPA00900 

SPA00910 

SPA00920 

SPA00930 

SPA00940 

SPA00950 

SPA00960 

SPA00970 

SPA00980 

SPA00990 

SPA01000 

SPA01010 

SPA01020 

SPA01030 

SPA01040 

SPA01050 
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PARAMETER  (JR=5243,  KR=5397,  MR=262139) 


MIN  =  15.  0 
WT=1.  ODOO 

PI  =  4. ODO  *  ATAN( 1.  ODO) 


«--i  .i_-t  .« . 

rcTcrrtrft 


READ  LAMA  AND  UGVEX  MATRICIES 


CALL  EXCMS  ( 'CLRSCRN' ) 

WRITE(6,1008) 

WRITE(6,*)  '  READING  LAMA  AND  UGVEX  MATRICIES' 

WRITEC6,*)  '  ' 

THIS  SECTION  READS  THE  LAMA  VECTOR  AND  THE  UGVEX 
MATRIX  AND  STORES  THEM  IN  MEMORY  FOR  FURTHER  RECALL  OF 
DESIRED  LOCATION  DATA. 

READ(4, 1001)  NAM 
READ( 4 , 1002 ) ( LAMA( I ) , 1=1 , 100 ) 

READ(4, 1001)  NAM 
DO  5  J  =  1,100 

READ(4, 1002)(UGVEX( I , J) , 1=1 ,684) 

CONTINUE 


FORMAT( IX, A6) 

FORM AT ( 1X.8E15. 8) 

FORMAT ( IX,////) 

CALL  EXCMS  ('CLRSCRN') 

************  STARTING  MODE  NUMBER 

**  SMODE  7  TO  100  (INTEGER)  **** 

SMODE=  17 


*********■****’* 


WRITE  (16,700)  SMODE 

FORMAT  ('  '.'STARTING  MODE  NUMBER:  ’,12) 

***************  NUMBER  OF  MODES  TO  SCAN  ************* 
**  MODE  1  TO  93  (INTEGER) 

M0DE=3 

EMODE  =  SMODE  +  MODE  -  1 
WRITE  (16,701)  MODE 

FORMAT  ('  'NUMBER  OF  MODES  SCANNED:  ’,12) 

************  NOISE  INPUT  POSITION  ***************** 
**  NODE  1  TO  114  (INTEGER)  (IF  0  THEN  NO  NOISE  INPUT) 

NODE=  8 


WRITE  (16,702)  NODE 


FORMAT  ( 


NOISE  NODE  LOCATION: 


'  ,15) 


SPA01060 

SPA01070 

SPA01080 

SPA01090 

SPA01100 

SPA01110 

SPA01120 

SPA01130 

SPA01140 

SPA01150 

SPA01160 

SPA01170 

SPA01180 

SPA01190 

SPA01200 

SPA01210 

SPA01220 

SPA01230 

SPA01240 

SPA01250 

SPA01260 

SPA01270 

SPA01280 

SPA01290 

SPA01300 

SPA01310 

SPA01320 

SPA01330 

SPA01340 

SPA01350 

SPA01360 

SPA01370 

SPA01380 

SPA01390 

SPA01400 

SPA01410 

SPA01420 

SPA01430 

SPA01440 

SPA01450 

SPA01460 

SPA01470 

SPA01480 

SPA01490 

SPA01500 

SPA01510 

SPA01520 

SPA01530 

SPA01540 

SPA01550 

SPA01560 

SPA01570 

SPA01580 

SPA01590 

SPA01600 

SPA01610 
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c 

c  ***********  START  AND  STOP  FOR  PLANT 
SN=17 
FN=4 

NS=SN*2-1 
NF=SN*2+2*FN-2 
WRITE  (16,899)  SN.FN 

899  FORMAT  ('  1 PLANT  --  SN=  ',15,'  FN=  ’,15) 

C  *************  SAMPLING  TIME  ************** 

C  **  sAMPT  MUST  BE  LESS  THAN  OR  EQUAL  TO  SAMPTM  ** 

SAMPT  =  0. 05 

SAMPTM  =  ((2.0D0*PI)/SQRT(LAMA(EM0DE)))/1.0D01 
IF  (SAMPT.  GE.  SAMPTM)  THEN 
SAMPT=SAMPTM 

ENDIF 

C 

WRITE  (16,900)  MIN 

900  FORMAT  ('  ' ,2X,'MIN:  ' ,F8. 3) 

C 

WRITE  (16,703)  SAMPT,  SAMPTM 

703  FORMAT  ('  SAMPLING  TIME:  ' ,D12. 4 , 2X, ' SAMPTM=  ' ,D15. 8) 

C 

q  ***************  DAMPING  FACTOR  ■  ************** 

C  **  DAMP  0.0  TO  1.0  (REAL* 8) 

DAMP=.  01 
C 

WRITE  (16,704)  DAMP 

704  FORMAT  ('  DAMPING  FACTOR:  ' ,D12. 4) 

C 

N0=3 

C  ***  PLANT  NOISE  VARIANCE  *** 

C  **  PNVARX,  PNVARY ,  PNVARZ  GT  0. 0 
C 

SF=1. ODO 
SF1=2. 5D06 
PNVARX=1.  0D00*SF1 
PNVARY=1.  0D00*SF1 
PNVARZ=1.  0D00--SF1 
C 
C 
C 

C  ***  MEASUREMENT  NOISE  VARIANCE  *** 

C  **  MNVARX,  MNVARY ,  MNVARZ  GT  0.  0 
MNVARX=1. 0D-03  *SF 
MNVARY=1.  0D-03  *SF 
MNVARZ=1.  0D-03  *SF 
C 
C 

WRITE  (16,711) 

711  FORMATC  ' , 'PLANT  NOISE  VARIANCE:  ’) 

WRITE  (16,712) 

712  FORMATC  ' ,6X, ' PNVARX' , 13X, ’ PNVARY* , 13X, ' PNVARZ’ ) 

WRITE  (16,713)  PNVARX,  PNVARY,  PNVARZ 

713  FORMATC  ' ,2X,E15. 8,2X,E15. 8, 2X,E15.  8) 

WRITE( 16,714) 

7 14  FORMAT( '  ' , ' MEASUREMENT  NOISE: ’ ) 


SPA01620 

SPA01630 

SPA01640 

SPA01650 

SPA01660 

SPA01670 

SPA01680 

SPA01690 

SPA01700 

SPA01710 

SPA01720 

SPA01730 

SPA01740 

SPA01750 

SPA01760 

SPA01770 

SPA01780 

SPA01790 

SPA01800 

SPA01810 

SPA01820 

SPA01830 

SPA01840 

SPA01850 

SPA01860 

SPA01870 

SPA01880 

SPA01890 

SPA01900 

SPA01910 

SPA01920 

SPA01930 

SPA01940 

SPA01950 

SPA01960 

SPA01970 

SPA01980 

SPA01990 

SPA02000 

SPA02010 

SPA02020 

SPA02030 

SPA02040 

SPA02050 

SPA02060 

SPA02070 

SPA02080 

SPA02090 

SPA02100 

SPA02110 

SPA02120 

SPA02130 

SPA02140 

SPA02150 

SPA02160 

SPA02170 


WRITE( 16,715) 

SPA02180 

715 

FORMATC  '  ,6X, 'MNVARX' , 13X, 'MNVARY' , 13X 

,'MNVARZ') 

SPA02190 

WRITE( 16,713)  MNVARX , MNVARY , MNVARZ 

SPA02200 

C 

SPA02210 

510 

CALL  EXCMS  ( 'CLRSCRN1 ) 

SPA02220 

WRITE  (6,1008) 

SPA02230 

WRITE  (6,*)  1 

PROGRAM  RUNNING’ 

SPA02240 

C 

SPA02250 

C 

***********  NOISE  INPUT  LOCATION 

wwwTfwirintTfTfwTr 

SPA02260 

C 

SPA02270 

R0WN3  =  N0DE*6 

SPA02280 

R0WN2  =  (NODE*6)  -  1 

SPA02290 

ROWN1  =  (NODE*6)  -  2 

SPA02300 

COUNT  =  0 

SPA02310 

C 

SPA02320 

C 

***********  INITIALIZE  MATRICIES 

**************** 

SPA02330 

C 

SPA02340 

DO  54  K  =  1,  200 

SPA02350 

X(K)  =0.0 

SPA02360 

54 

CONTINUE 

SPA02370 

C 

SPA02380 

DO  60  1=1,3 

SPA02390 

DO  58  J=1 , 3 

SPA02400 

RMN( I , J)=0.  0 

SPA02410 

QPN( I , J)=0. 0 

SPA02420 

58 

CONTINUE 

SPA02430 

60 

CONTINUE 

SPA02440 

C 

SPA02450 

RMN( 1 , 1 )=MNVARX**2.  0 

SPA02460 

RMN (2,2) =MNVAR Y**2 .  0 

SPA02470 

RMN( 3 , 3 )=MNVARZ**2 .  0 

SPA02480 

QPN( 1, 1)=PNVARX**2.  0 

SPA02490 

QPN( 2 ,  2)=PNVARY**2.  0 

SPA02500 

QPN( 3 , 3)=PNVARZ**2.  0 

SPA02510 

C 

SPA02520 

C 

*********************************************************** 

SPA02530 

C 

*****  BEGIN  MAIN  PROGRAM 

***** 

SPA02540 

C 

SPA02550 

C 

SPA02560 

CALL  STMTRX( EMODE , SMODE , S AMPT , DAMP , PHI , 1 

GAMMA , A , B , LAMA , UGVEX , C , 

SPA02570 

+•  ROWN 1 ,  R0WN2 ,  R0WN3 ,  BN ) 

SPA02580 

C 

SPA02590 

C 

SPA02600 

WRITE  (6,1008) 

SPA02610 

WRITE(6,*)  '  EXIT  STMTRX  -  -  -  PRE-LOOP 

KALMAN' 

SPA02620 

C 

SPA02630 

C 

SPA02640 

WRITE  (6,*)  'COMPUTING  C  TIMES  SF  FOR  NEW  C' 

SPA02650 

C 

SPA02660 

WRITE  (16,1008) 

SPA02670 

DO  11000  1=1,200 

SPA02680 

DO  10900  J=1 ,N0 

SPA02690 

C(J,I)=  C(  J, I)*SF 

SPA02700 

10900 

CONTINUE 

SPA02710 

11000 

CONTINUE 

SPA02720 

C 

SPA02730 
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***  PRE-LOOP  PORTION  OF  KALMAN  FILTER 


JK=SMODE*2-2 

M2=2*M0DE 


M2=2*M0DE 

JP=2*SM0DE-1 

JQ=2*EM0DE 


DO  8813  1=1,3 
EX(I)=0.  0 
813  CONTINUE 


********************************************************** 

*****  THIS  SECTION  COMPUTES  THE  STATE  UPDATE  ***** 
********************************************************** 

ESS  =0.0 
COUNT  =  0 
ENERGY  =  0.  0D0 
TIME  =  0.0 
CGN=0.  0 
CTG=0.  0 

*****  SETS  LOOP  FOR  THE  ITERATIONS  NECESSARY  TO  OBSERVE  ***** 
*****  the  SYSTEM  FOR  THE  NUMBER  OF  MINUTES  SPECIFIED  ***** 
WRITE  (6,1008) 

WRITE  (6,*)  '  START  STATE  UPDATE  ' 

LOOP  =  INT((MIN*60.  0)/SAMPT) 

PRT=  (DBLE(LOOP) )/30.  0 
PRT1=(DBLE(L00P) )/50.  00 
CTT=0.  0 

DO  400  L  =0,  LOOP 
TIME  =  DBLE(L)*SAMPT 

IF(L.  EQ.  0)THEN 

IMPLSE  =(1.  0D06*SF1)/(DSQRT(SAMPT) ) 

ELSE 

IMPLSE  =  0.  0D0 
ENDIF 

T0=0. 0 

*****  RANDOM  NUMBER  GENERATOR  ***** 


DO  101  1=1,6 

I SEED=M0D( I SEED* JR+KR , MR ) 
RND1=(DBLE( ISEED)+0.  5DOO)/DBLE(MR) 
ISEED=M0D( ISEED*JR+KR , MR) 
RND2=(DBLE( ISEED)+0.  5D00)/DBLE(MR) 


SPA02740 
SPA02750 
SPA02760 
SPA02770 
SPA02780 
SPA02790 
SPA02800 
SPA02810 
SPA02820 
SPA02830 
SPA02840 
SPA02850 
SPA02860 
SPA02870 
SPA02880 
SPA02890 
SPA02900 
SPA029 10 
SPA02920 
SPA02930 
SPA02940 
SPA02950 
SPA02960 
SPA02970 
SPA02980 
SPA02990 
SPA03000 
SPA03010 
SPA03020 
SPA03030 
SPA03040 
SPA03050 
SPA03060 
SPA03070 
SPA03080 
SPA03090 
SPA03100 
SPA03110 
SPA03120 
SPA03130 
SPA03140 
SPA03150 
SPA03160 
SPA03170 
SPA03180 
SPA03190 
SPA03200 
SPA03210 
SPA03220 
SPA03230 
SPA03240 
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SPA03260 
SPA03270 
SPA03280 
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RNDM(I)=DSQRT(-2.  0*DLOG( RND 1 ) ) *DC OS ( 6 .  2831853D00*RND2) 

SPA03300 

101 

CONTINUE 

SPA03310 

c 

******************* 

SPA03320 

CTT=CTT+1.  0 

SPA03330 

c 

****  START  OF  STATE  UPDATE  *** 

SPA03340 

c 

SPA03350 

c 

***  COMPUTE  AX°200  =  A®200  X  200  *  X®200 

SPA03360 

c 

SPA03370 

c 

SPA03380 

c 

***  COMPUTE  AXH  =  A*XH 

SPA03390 

c 

SPA03400 

JK=SM0DE*2-2 

SPA03410 

JP=JK+1 

SPA03420 

JQ=2*EM0DE 

SPA03430 

c 

SPA03440 

c 

SPA03450 

DO  5015  I=NS,NF 

SPA03460 

SUM=0.  0 

SPA03470 

DO  5010  K=NS , NF 

SPA03480 

SUM=SUM+A( I ,K)*X(K) 

SPA03490 

5010 

CONTINUE 

SPA03500 

AX(I)=SUM 

SPA03510 

5015 

CONTINUE 

SPA03520 

C 

SPA03530 

C 

SPA03540 

C 

***  COMPUTE  WD°3 

SPA03550 

C 

SPA03560 

WD( 1)=PNVARX*RNDM( 1)*T0+IMPLSE 

SPA03570 

WD( 2)=PNVARY*RNDM(2)*TO 

SPA03580 

WD( 3 ) =PNVARZ*RNDM ( 3 )*T0 

SPA03590 

C 

SPA03600 

C 

***  COMPUTE  BNWD°200  =BN°200  X  3  *  WD°3 

SPA03610 

C 

SPA03620 

DO  5035  I=NS,NF 

SPA03630 

SUM=0. 0 

SPA03640 

DO  5030  K=1 , 3 

SPA03650 

SUM=SUM+BN(I ,K)*WD(K) 

SPA03660 

5030 

CONTINUE 

SPA03670 

BNVD(I)=SUM 

SPA03680 

5035 

CONTINUE 

SPA03690 

C 

SPA03700 

C 

***  COMPUTE  X°200  =AX°200  +  BNWD°200 

SPA03710 

C 

SPA03720 

DO  5040  I=NS ,NF 

SPA03730 

X(I)=  AX( I)  +  BNWD(I) 

SPA03740 

IF  (DABSCX(I)). LT.  1.0D-60)  THEN 

SPA03750 

X(I)=1.  0D-60 

SPA03760 

END  IF 

SPA03770 

C 

SPA03780 

5040 

CONTINUE 

SPA03790 

C 

SPA03800 

C 

SPA03810 

C 

******************* 

SPA03820 

C 

SPA03830 

C 

***  START  OF  KALMAN  FILTER  *** 

SPA03840 

C 

SPA03850 
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JK=SM0DE*2-2 

SPA03860 

JP=JK+1 

SPA03870 

JQ=2*EMODE 

SPA03880 

M2=2*M0DE 

SPA03890 

c 

SPA03900 

JL=JQ+1 

SPA03910 

DO  8888  1=1, NO 

SPA03920 

SUM=0.  0 

SPA03930 

DO  8887  K=JL,NF 

SPA03940 

SUM=SUM+C( I ,K)*X(K) 

SPA03950 

8887 

CONTINUE 

SPA03960 

EX( I ) =SUM*SUM*S AMPT+EX ( I ) 

SPA03970 

8888 

CONTINUE 

SPA03980 

C 

SPA03990 

C 

SPA04000 

CGN=CGN+1.  0 

SPA04010 

IF  (CTT.EQ.  1.  0.  OR.  CGN.GT.  PRT)  THEN 

SPA04020 

C 

SPA04030 

WRITE  (16,1008') 

SPA04040 

WRITE  (16,*)  'TIME  =  ’,  TIME 

SPA04050 

C 

SPA04060 

DO  380  I=JP  ,  JQ 

SPA04070 

WRITE  (16,4500)  I,X(I) 

SPA04080 

380 

CONTINUE 

SPA04090 

4500 

FORMAT  (’  ',2X,’X(,,I4,')=  ’.D15.8) 

SPA04100 

C 

SPA04110 

CGN=0.  0 

SPA04120 

END  IF 

SPA04130 

C 

SPA04140 

C 

SPA04150 

4  00 

CONTINUE 

SPA04160 

WRITE  (11,*)  'SMODE  =  ',  SMODE 

SPA04170 

WRITE  (11,*)  'EMODE  =  ',  EMODE 

SPA04180 

WRITE  (11,*)  'SN  =  ' , SN 

SPA04190 

WRITE  (11,*)  'FN  =  ' ,FN 

SPA04200 

SPA04210 

C 

SPA04220 

JL=JQ+1 

SPA04230 

DO  9499  1=1, NO 

SPA04240 

WRITE  (11,*)  'EX  ',1  ,'  ' ,  EX( I) 

SPA04250 

9499 

CONTINUE 

SPA04260 

C 

SPA04270 

C 

SPA04280 

C 

SPA04290 

C 

SPA04300 

C 

SPA04310 

CALL  EXCMS  ( ’CLRSCRN' ) 

SPA04320 

WRITE  (6,1008) 

SPA04330 

C 

SPA04340 

599 

STOP 

SPA04350 

END 

SPA04360 

C 

SPA04370 

C 

SPA04380 

C 

SPA04390 

C 

SPA04400 

C 

SPA04410 
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c 

c 

c 

c 

c 


c 


c 

c 

c 

c 


c 

c 

c 

c 


c 

c 

c 

c 


c 

c 

c 

c 


c 

c 

c 

c 


THIS  SUBROUTINE  COMPUTES  THE  STATE  TRANSITION  MATRIX  FOR  EACH 
OF  THE  100  MODES 


SUBROUTINE  STMTRXC EMODE , SMODE ,T , D , PHI , GAMMA , A , B , LAMA , UGVEX , C , 
+  ROWN 1 , ROWN2 , ROWN3 , BN) 

REAL*8  WN,GMA,PHI(2,2J100),GAMMA(2,100),EGT>T,COSW1T,SINW1T 
REAL*8  W1 ,D,A(200,200) ,B(200, 3) ,C(  9 , 200) ,BN( 200,3) 

REAL  LAMA( 100) ,UGVEX(684, 100) 

INTEGER  SMODE, R, EMODE, JJ,KK, ROWN 1 , R0WN2 , ROWN3 ,  NN(9),  N9,  NO 


WRITE  (6,*)  'INSIDE  STMTRX  -  -  COMPUTE  WN,  GMA,  EFT,  Wl' 


DO  600  I  =  1  ,100 

WN  =  DBLE( SQRT( LAMA( I ) ) ) 

GMA  =  D*WN/2. 0 
EGT  =  DEXP( -GMA*T) 

Wl  ■  DSQRT( (WN**2) -(GMA**2) ) 
COSW1T  =  DC0S(W1*T) 

SINW1T  =  DSIN(W1*T) 


IF(WN.  EQ.  0)THEN 

PHI (1,1, I)  ■  EGT*C0SW1T 
PHI( 1,2,1)  =  T 
PHI(2, 1,1)  =  0 
PHI (2, 2, I)  =  EGT*C0SW1T 


GAMMA( 1,1)  =  0 
GAMMA ( 2,1)  =  0 

ELSE 


PHI( 1,1,1)  =  EGT*(C0SW1T  +  (GMA*(W1**(-1)))*SINW1T) 

PHI( 1,2,1)  =  (Wl**( -1) )*EGT*SINW1T 

PHI(2, 1,1)  =  -(WN**2)*(W1**(-1))*EGT*SINW1T 

PHI (2, 2, I)  =  EGT*(C0SW1T  -  (GMA*(W1**( -1) ) )*SINW1T) 


GAMMA( 1,I)=(WN**( -2) )*( 1. ODO-EGT*COSW1T  -EGT*(GMA/W1)*SINW1T) 


SPA04420 

SPA04430 

SPA04440 

SPA04450 

SPA04460 

SPA04470 

SPA04480 

SPA04490 

SPA04500 

SPA04510 

SPA04520 

SPA04530 

SPA04540 

SPA04550 

SPA04560 

SPA04570 

SPA04580 

SPA04590 

SPA04600 

SPA04610 

SPA04620 

SPA04630 

SPA04640 

SPA04650 

SPA04660 

SPA04670 

SPA04680 

SPA04690 

SPA04700 

SPA04710 

SPA04720 

SPA04730 

SPA04740 

SPA04750 

SPA04760 

SPA04770 

SPA04780 

SPA04790 

SPA04800 

SPA04810 

SPA04820 

SPA04830 

SPA04840 

SPA04850 

SPA04860 

SPA04870 

SPA04880 

SPA04890 

SPA04900 

SPA04910 

SPA04920 

SPA04930 

SPA04940 

SPA04950 

SPA04960 


y 
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GAMMA( 2,1)  =  (W1**(-1))*EGT*SINW1T 

SPA04970 

c 

SPA04980 

c 

SPA04990 

c 

SPA05000 

c 

SPA05010 

V 

ENDIF 

SPA05020 

c 

SPA05030 

c 

SPA05040 

c 

SPA05050 

600 

CONTINUE 

SPA05060 

c 

SPA05070 

WRITE  (6,*)  'PHI  AND  GAMMA  COMPUTED' 

SPA05080 

WRITE  (6,*)  '  COMPUTING  A,  B,  BN' 

SPA05090 

c 

SPA05100 

R  =  1 

SPA05110 

c 

SPA05120 

DO  610  K  *  1  ,100 

SPA05130 

c 

SPA05140 

c 

SPA05150 

c 

SPA05160 

c 

SPA05170 

c 

SPA05180 

A(R,R)  =  PHI( 1 , 1 ,K) 

SPA05190 

A(R,R+1)  =  PHI( 1,2 ,K) 

SPA05200 

A(R+1,R)  =  PHI( 2 , 1 ,K) 

SPA05210 

A(R+1,R+1)  =  PHI(2,2,K) 

SPA05220 

c 

SPA05230 

c 

SPA05240 

c 

SPA05250 

c 

SPA05260 

V 

c 

SPA05270 

c 

***  B  MATRIX  FOR  MULTIPLYING  CONTROL  TORQUES 

SPA05280 

• 

c 

SPA05290 

B(R, 1)  =  GAMMA(1,K)*DBLE(UGVEX(412,K)) 

SPA05300 

i 

B(R,2)  =  GAMMA( 1 ,K)*DBLE(UGVEX(413 ,K) ) 

SPA05310 

B(R,3)  =  GAMMA( 1 ,K)*DBLE(UGVEX(414,K) ) 

SPA05320 

B(R+1 , 1)  =  GAMMA( 2 ,K)*DBLE(UGVEX(412 ,K) ) 

SPA05330 

B(R+1,2)  =  GAMMA( 2 ,K)*DBLE(UGVEX(413 ,K) ) 

SPA05340 

BCR+1,3)  =  GAMMA(2 ,K)*DBLE(UGVEX(414,K) ) 

SPA05350 

c 

SPA05360 

c 

SPA05370 

c 

SPA05380 

c 

SPA05390 

c 

SPA05400 

c 

SPA05410 

c 

SPA05420 

c 

***  BN  MATRIX  FOR  MULTIPLYING  THE  NOISE  DISTURBANCES 

SPA05430 

c 

SPA05440 

c 

SPA05450 

c 

SPA05460 

c 

SPA05470 

BN ( R , 1 ) =GAMMA ( 1 , K ) *DB LE ( UGVEX( ROWN 1 , K ) ) 

SPA05480 

ft 

BN(R,2)=GAMMA( 1 ,K)*DBLE(UGVEX(R0WN2 ,K) ) 

SPA05490 

BN(R, 3)=GAMMA( 1 ,K)*DBLE(UGVEX(R0WN3 ,K) ) 

SPA05500 

BN( R+ 1 , 1 ) =GAMMA ( 2 , K ) *DBLE ( UGVEX( ROWN 1 , K ) ) 

SPA05510 

% 

BN(R+1 ,2) =GAMMA( 2 , K) *DBLE( UGVEX ( R0WN2 , K ) ) 

SPA05520 
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BN( R+l , 3 ) =GAMMA( 2 , K)*DBLE ( UGVEX( ROWN3 , K) ) 

SPA05530 

c 

SPA05540 

c 

SPA05550 

c 

SPA05560 

c 

SPA05570 

c 

SPA05580 

c 

SPA05590 

R  =  R+2 

SPA05600 

610 

CONTINUE 

SPA05610 

C 

SPA05620 

WRITE  (6,*)  'A,  B,  BN  COMPUTED' 

SPA05630 

C 

SPA05640 

WRITE  (6,*)  'COMPUTING  C’ 

SPA05650 

c 

SPA05660 

c 

SPA05670 

c 

SPA05680 

c 

***  C  MATRIX  PRODUCTION  *** 

SPA05690 

N0=3 

SPA05700 

NN( 1)=418 

SPA05710 

NN( 2)=419 

SPA05720 

NN( 3)=420 

SPA05730 

c 

SPA05740 

c 

SPA05750 

c 

SPA05760 

c 

SPA05770 

c 

SPA05780 

c 

SPA05790 

JJ=-1 

SPA05800 

DO  640  1=1,100 

SPA05810 

JJ=JJ+1 

SPA05820 

c 

SPA05830 

DO  9127  K=1 ,NO 

SPA05840 

c 

SPA05850 

KK=I+JJ 

SPA05860 

c 

SPA05870 

N9=NN(K) 

SPA05880 

c 

SPA05890 

C(K,KK)  =  DBLE ( UGVEX( N9 , I ) ) 

SPA05900 

c 

SPA05910 

KK=KK+1 

SPA05920 

c 

SPA05930 

C(K,KK)=0.  0 

SPA05940 

9127 

CONTINUE 

SPA05950 

640 

CONTINUE 

SPA05960 

C 

SPA05970 

C 

SPA05980 

C 

SPA05990 

RETURN 

SPA06000 

END 

SPA06010 
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